gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp

       1  // Copyright (  c ) 2013,   Open Source Robotics Foundation. All rights reserved.
          // Copyright (  c ) 2013,   The Johns Hopkins University. All rights reserved.
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          //
          // * Redistributions in binary form must reproduce the above copyright
          // notice,   this list of conditions and the following disclaimer in the
          // documentation and/or other materials provided with the distribution.
          //
          // * Neither the name of the Open Source Robotics Foundation nor the names of its
          // contributors may be used to endorse or promote products derived from
          // this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
          // AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
          // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
          // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
          // LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
          // CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
          // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
          // INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
          // CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
          // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          /* Author: Dave Coleman,   Jonathan Bohren
           Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
           using pluginlib
          */
          
          #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
          #define GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_manager/controller_manager.hpp"
          
          #include "gazebo/common/common.hh"
          #include "gazebo/physics/Model.hh"
          
          namespace gazebo_ros2_control
          {
      49  class GazeboRosControlPrivate;
          
      51  class GazeboRosControlPlugin : public gazebo::ModelPlugin
          {
          public:
      54   GazeboRosControlPlugin(   );
          
      56   ~GazeboRosControlPlugin(   );
          
           // Overloaded Gazebo entry point
           void Load(  gazebo::physics::ModelPtr parent,   sdf::ElementPtr sdf ) override;
          
          private:
           /// Private data pointer
           std::unique_ptr<GazeboRosControlPrivate> impl_;
          };
          } // namespace gazebo_ros2_control
          
          #endif // GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_

gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp

       1  // Copyright 2021 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          
          #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
          #define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "angles/angles.h"
          
          #include "gazebo_ros2_control/gazebo_system_interface.hpp"
          
          #include "std_msgs/msg/bool.hpp"
          
          namespace gazebo_ros2_control
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          // Forward declaration
      34  class GazeboSystemPrivate;
          
          // These class must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a
          // simulated `ros2_control` `hardware_interface::SystemInterface`.
          
      39  class GazeboSystem : public GazeboSystemInterface
          {
          public:
           // Documentation Inherited
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & system_info )
           override;
          
           // Documentation Inherited
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override;
          
           // Documentation Inherited
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override;
          
           // Documentation Inherited
           CallbackReturn on_activate(  const rclcpp_lifecycle::State & previous_state ) override;
          
           // Documentation Inherited
           CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & previous_state ) override;
          
           // Documentation Inherited
           hardware_interface::return_type read(  
           const rclcpp::Time & time,  
           const rclcpp::Duration & period ) override;
          
           // Documentation Inherited
           hardware_interface::return_type write(  
           const rclcpp::Time & time,  
           const rclcpp::Duration & period ) override;
          
           // Documentation Inherited
           bool initSim(  
           rclcpp::Node::SharedPtr & model_nh,  
           gazebo::physics::ModelPtr parent_model,  
           const hardware_interface::HardwareInfo & hardware_info,  
           sdf::ElementPtr sdf ) override;
          
          private:
           void registerJoints(  
           const hardware_interface::HardwareInfo & hardware_info,  
           gazebo::physics::ModelPtr parent_model );
          
           void registerSensors(  
           const hardware_interface::HardwareInfo & hardware_info,  
           gazebo::physics::ModelPtr parent_model );
          
           /// \brief Private data class
           std::unique_ptr<GazeboSystemPrivate> dataPtr;
          };
          
          } // namespace gazebo_ros2_control
          
          #endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_

gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp

       1  // Copyright 2021 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          
          #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
          #define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gazebo/physics/Joint.hh"
          #include "gazebo/physics/Model.hh"
          #include "gazebo/physics/physics.hh"
          
          #include "hardware_interface/system_interface.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          
          namespace gazebo_ros2_control
          {
          
          template<class ENUM,   class UNDERLYING = typename std::underlying_type<ENUM>::type>
      36  class SafeEnum
          {
          public:
      39   SafeEnum(   )
           : mFlags(  0 ) {}
      41   explicit SafeEnum(  ENUM singleFlag )
           : mFlags(  singleFlag ) {}
      43   SafeEnum(  const SafeEnum & original )
           : mFlags(  original.mFlags ) {}
          
      46   SafeEnum & operator|=(  ENUM addValue ) {mFlags |= addValue; return *this;}
      47   SafeEnum operator|(  ENUM addValue ) {SafeEnum result(  *this ); result |= addValue; return result;}
      48   SafeEnum & operator&=(  ENUM maskValue ) {mFlags &= maskValue; return *this;}
      49   SafeEnum operator&(  ENUM maskValue ) {SafeEnum result(  *this ); result &= maskValue; return result;}
      50   SafeEnum operator~(   ) {SafeEnum result(  *this ); result.mFlags = ~result.mFlags; return result;}
      51   explicit operator bool(   ) {return mFlags != 0;}
          
          protected:
      54   UNDERLYING mFlags;
          };
          
          // SystemInterface provides API-level access to read and command joint properties.
      58  class GazeboSystemInterface
      59   : public hardware_interface::SystemInterface
          {
          public:
           /// \brief Initilize the system interface
           /// param[in] model_nh pointer to the ros2 node
           /// param[in] parent_model pointer to the model
           /// param[in] control_hardware vector filled with information about robot's control resources
           /// param[in] sdf pointer to the SDF
      67   virtual bool initSim(  
      68   rclcpp::Node::SharedPtr & model_nh,  
      69   gazebo::physics::ModelPtr parent_model,  
           const hardware_interface::HardwareInfo & hardware_info,  
      71   sdf::ElementPtr sdf ) = 0;
          
           // Methods used to control a joint.
           enum ControlMethod_
           {
           NONE = 0,  
           POSITION = (  1 << 0 ),  
           VELOCITY = (  1 << 1 ),  
           EFFORT = (  1 << 2 ),  
           };
          
           typedef SafeEnum<enum ControlMethod_> ControlMethod;
          
          protected:
      85   rclcpp::Node::SharedPtr nh_;
          };
          
          } // namespace gazebo_ros2_control
          
          #endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_

gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp

       1  // Copyright (  c ) 2013,   Open Source Robotics Foundation. All rights reserved.
          // Copyright (  c ) 2013,   The Johns Hopkins University. All rights reserved.
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          //
          // * Redistributions in binary form must reproduce the above copyright
          // notice,   this list of conditions and the following disclaimer in the
          // documentation and/or other materials provided with the distribution.
          //
          // * Neither the name of the Open Source Robotics Foundation nor the names of its
          // contributors may be used to endorse or promote products derived from
          // this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
          // AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
          // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
          // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
          // LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
          // CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
          // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
          // INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
          // CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
          // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          /* Author: Dave Coleman,   Jonathan Bohren
           Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
           using pluginlib
          */
          
          #include <string>
          #include <memory>
          #include <utility>
          #include <vector>
          
          #include "gazebo_ros/node.hpp"
          
          #include "gazebo_ros2_control/gazebo_ros2_control_plugin.hpp"
          #include "gazebo_ros2_control/gazebo_system.hpp"
          
          #include "pluginlib/class_loader.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "hardware_interface/resource_manager.hpp"
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          #include "yaml-cpp/yaml.h"
          
          using namespace std::chrono_literals;
          
          namespace gazebo_ros2_control
          {
          
      60  class GazeboRosControlPrivate
          {
          public:
           GazeboRosControlPrivate(   ) = default;
          
           virtual ~GazeboRosControlPrivate(   ) = default;
          
           // Called by the world update start event
           void Update(   );
          
           // Called on world reset
           virtual void Reset(   );
          
           // Get the URDF XML from the parameter server
           std::string getURDF(  std::string param_name ) const;
          
           // Node Handles
           gazebo_ros::Node::SharedPtr model_nh_;
          
           // Pointer to the model
           gazebo::physics::ModelPtr parent_model_;
          
           // Pointer to the update event connection
           gazebo::event::ConnectionPtr update_connection_;
          
           // Interface loader
           boost::shared_ptr<pluginlib::ClassLoader<
           gazebo_ros2_control::GazeboSystemInterface>> robot_hw_sim_loader_;
          
           // String with the robot description
           std::string robot_description_;
          
           // String with the name of the node that contains the robot_description
           std::string robot_description_node_;
          
           // Name of the file with the controllers configuration
           std::string param_file_;
          
           // Executor to spin the controller
           rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
          
           // Thread where the executor will sping
           std::thread thread_executor_spin_;
          
           // Flag to stop the executor thread when this plugin is exiting
           bool stop_;
          
           // Controller manager
           std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
          
           // Available controllers
           std::vector<std::shared_ptr<controller_interface::ControllerInterface>> controllers_;
          
           // Timing
           rclcpp::Duration control_period_ = rclcpp::Duration(  1,   0 );
          
           // Last time the update method was called
           rclcpp::Time last_update_sim_time_ros_ = rclcpp::Time(  (  int64_t )0,   RCL_ROS_TIME );
          };
          
     120  GazeboRosControlPlugin::GazeboRosControlPlugin(   )
          : impl_(  std::make_unique<GazeboRosControlPrivate>(   ) )
          {
          }
          
     125  GazeboRosControlPlugin::~GazeboRosControlPlugin(   )
          {
           // Stop controller manager thread
           impl_->stop_ = true;
           impl_->executor_->remove_node(  impl_->controller_manager_ );
           impl_->executor_->cancel(   );
           impl_->thread_executor_spin_.join(   );
          
           // Disconnect from gazebo events
           impl_->update_connection_.reset(   );
          }
          
          // Overloaded Gazebo entry point
     138  void GazeboRosControlPlugin::Load(  gazebo::physics::ModelPtr parent,   sdf::ElementPtr sdf )
          {
           RCLCPP_INFO_STREAM(  
           rclcpp::get_logger(  "gazebo_ros2_control" ),  
           "Loading gazebo_ros2_control plugin" );
          
           // Save pointers to the model
           impl_->parent_model_ = parent;
          
           // Get parameters/settings for controllers from ROS param server
           // Initialize ROS node
           impl_->model_nh_ = gazebo_ros::Node::Get(  sdf );
          
           RCLCPP_INFO(  
           impl_->model_nh_->get_logger(   ),   "Starting gazebo_ros2_control plugin in namespace: %s",  
           impl_->model_nh_->get_namespace(   ) );
          
           RCLCPP_INFO(  
           impl_->model_nh_->get_logger(   ),   "Starting gazebo_ros2_control plugin in ros 2 node: %s",  
           impl_->model_nh_->get_name(   ) );
          
           // Error message if the model couldn't be found
           if (  !impl_->parent_model_ ) {
           RCLCPP_ERROR_STREAM(  impl_->model_nh_->get_logger(   ),   "parent model is NULL" );
           return;
           }
          
           // Check that ROS has been initialized
           if (  !rclcpp::ok(   ) ) {
           RCLCPP_FATAL_STREAM(  
           impl_->model_nh_->get_logger(   ),  
           "A ROS node for Gazebo has not been initialized,   unable to load plugin. " <<
           "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package )" );
           return;
           }
          
           // Get robot_description ROS param name
           if (  sdf->HasElement(  "robot_param" ) ) {
           impl_->robot_description_ = sdf->GetElement(  "robot_param" )->Get<std::string>(   );
           } else {
           impl_->robot_description_ = "robot_description"; // default
           }
          
           // Get robot_description ROS param name
           if (  sdf->HasElement(  "robot_param_node" ) ) {
           impl_->robot_description_node_ =
           sdf->GetElement(  "robot_param_node" )->Get<std::string>(   );
           } else {
           impl_->robot_description_node_ = "robot_state_publisher"; // default
           }
          
           if (  sdf->HasElement(  "parameters" ) ) {
           impl_->param_file_ = sdf->GetElement(  "parameters" )->Get<std::string>(   );
           RCLCPP_INFO(  
           impl_->model_nh_->get_logger(   ),   "Loading parameter file %s\n",   impl_->param_file_.c_str(   ) );
           } else {
           RCLCPP_ERROR(  
           impl_->model_nh_->get_logger(   ),   "No parameter file provided. Configuration might be wrong" );
           }
          
           // There's currently no direct way to set parameters to the plugin's node
           // So we have to parse the plugin file manually and set it to the node's context.
           auto rcl_context = impl_->model_nh_->get_node_base_interface(   )->get_context(   )->get_rcl_context(   );
           std::vector<std::string> arguments = {"--ros-args",   "--params-file",   impl_->param_file_.c_str(   )};
           if (  sdf->HasElement(  "ros" ) ) {
           sdf = sdf->GetElement(  "ros" );
          
           // Set namespace if tag is present
           if (  sdf->HasElement(  "namespace" ) ) {
           std::string ns = sdf->GetElement(  "namespace" )->Get<std::string>(   );
           // prevent exception: namespace must be absolute,   it must lead with a '/'
           if (  ns.empty(   ) || ns[0] != '/' ) {
           ns = '/' + ns;
           }
           std::string ns_arg = std::string(  "__ns:=" ) + ns;
           arguments.push_back(  RCL_REMAP_FLAG );
           arguments.push_back(  ns_arg );
           }
          
           // Get list of remapping rules from SDF
           if (  sdf->HasElement(  "remapping" ) ) {
           sdf::ElementPtr argument_sdf = sdf->GetElement(  "remapping" );
          
           arguments.push_back(  RCL_ROS_ARGS_FLAG );
           while (  argument_sdf ) {
           std::string argument = argument_sdf->Get<std::string>(   );
           arguments.push_back(  RCL_REMAP_FLAG );
           arguments.push_back(  argument );
           argument_sdf = argument_sdf->GetNextElement(  "remapping" );
           }
           }
           }
          
           std::vector<const char *> argv;
           for (  const auto & arg : arguments ) {
           argv.push_back(  reinterpret_cast<const char *>(  arg.data(   ) ) );
           }
           rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments(   );
           rcl_ret_t rcl_ret = rcl_parse_arguments(  
           static_cast<int>(  argv.size(   ) ),  
           argv.data(   ),   rcl_get_default_allocator(   ),   &rcl_args );
           rcl_context->global_arguments = rcl_args;
           if (  rcl_ret != RCL_RET_OK ) {
           RCLCPP_ERROR(  impl_->model_nh_->get_logger(   ),   "parser error %s\n",   rcl_get_error_string(   ).str );
           rcl_reset_error(   );
           return;
           }
           if (  rcl_arguments_get_param_files_count(  &rcl_args ) < 1 ) {
           RCLCPP_ERROR(  
           impl_->model_nh_->get_logger(   ),   "failed to parse yaml file: '%s'\n",  
           impl_->param_file_.c_str(   ) );
           return;
           }
          
           // Get the Gazebo simulation period
           rclcpp::Duration gazebo_period(  
           std::chrono::duration_cast<std::chrono::nanoseconds>(  
           std::chrono::duration<double>(  
           impl_->parent_model_->GetWorld(   )->Physics(   )->GetMaxStepSize(   ) ) ) );
          
           // Read urdf from ros parameter server then
           // setup actuators and mechanism control node.
           // This call will block if ROS is not properly initialized.
           std::string urdf_string;
           std::vector<hardware_interface::HardwareInfo> control_hardware;
           try {
           urdf_string = impl_->getURDF(  impl_->robot_description_ );
           control_hardware = hardware_interface::parse_control_resources_from_urdf(  urdf_string );
           } catch (  const std::runtime_error & ex ) {
           RCLCPP_ERROR_STREAM(  
           impl_->model_nh_->get_logger(   ),  
           "Error parsing URDF in gazebo_ros2_control plugin,   plugin not active : " << ex.what(   ) );
           return;
           }
          
           std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
           std::make_unique<hardware_interface::ResourceManager>(   );
          
           try {
           impl_->robot_hw_sim_loader_.reset(  
           new pluginlib::ClassLoader<gazebo_ros2_control::GazeboSystemInterface>(  
           "gazebo_ros2_control",  
           "gazebo_ros2_control::GazeboSystemInterface" ) );
           } catch (  pluginlib::LibraryLoadException & ex ) {
           RCLCPP_ERROR(  
           impl_->model_nh_->get_logger(   ),   "Failed to create robot simulation interface loader: %s ",  
           ex.what(   ) );
           }
          
           for (  unsigned int i = 0; i < control_hardware.size(   ); i++ ) {
           std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type;
           auto gazeboSystem = std::unique_ptr<gazebo_ros2_control::GazeboSystemInterface>(  
           impl_->robot_hw_sim_loader_->createUnmanagedInstance(  robot_hw_sim_type_str_ ) );
          
           rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(  impl_->model_nh_ );
           if (  !gazeboSystem->initSim(  
           node_ros2,  
           impl_->parent_model_,  
           control_hardware[i],  
           sdf ) )
           {
           RCLCPP_FATAL(  
           impl_->model_nh_->get_logger(   ),   "Could not initialize robot simulation interface" );
           return;
           }
          
           resource_manager_->import_component(  std::move(  gazeboSystem ),   control_hardware[i] );
           }
          
           rclcpp_lifecycle::State state(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           hardware_interface::lifecycle_state_names::ACTIVE );
           resource_manager_->set_component_state(  "GazeboSimSystem",   state );
          
           impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(   );
          
           // Create the controller manager
           RCLCPP_INFO(  impl_->model_nh_->get_logger(   ),   "Loading controller_manager" );
           impl_->controller_manager_.reset(  
           new controller_manager::ControllerManager(  
           std::move(  resource_manager_ ),  
           impl_->executor_,  
           "controller_manager" ) );
           impl_->executor_->add_node(  impl_->controller_manager_ );
          
           if (  !impl_->controller_manager_->has_parameter(  "update_rate" ) ) {
           RCLCPP_ERROR_STREAM(  
           impl_->model_nh_->get_logger(   ),   "controller manager doesn't have an update_rate parameter" );
           return;
           }
          
           auto cm_update_rate = impl_->controller_manager_->get_parameter(  "update_rate" ).as_int(   );
           impl_->control_period_ = rclcpp::Duration(  
           std::chrono::duration_cast<std::chrono::nanoseconds>(  
           std::chrono::duration<double>(  1.0 / static_cast<double>(  cm_update_rate ) ) ) );
           // Check the period against the simulation period
           if (  impl_->control_period_ < gazebo_period ) {
           RCLCPP_ERROR_STREAM(  
           impl_->model_nh_->get_logger(   ),  
           "Desired controller update period (  " << impl_->control_period_.seconds(   ) <<
           " s ) is faster than the gazebo simulation period (  " <<
           gazebo_period.seconds(   ) << " s )." );
           } else if (  impl_->control_period_ > gazebo_period ) {
           RCLCPP_WARN_STREAM(  
           impl_->model_nh_->get_logger(   ),  
           " Desired controller update period (  " << impl_->control_period_.seconds(   ) <<
           " s ) is slower than the gazebo simulation period (  " <<
           gazebo_period.seconds(   ) << " s )." );
           }
          
           impl_->stop_ = false;
           auto spin = [this](   )
           {
           while (  rclcpp::ok(   ) && !impl_->stop_ ) {
           impl_->executor_->spin_once(   );
           }
           };
           impl_->thread_executor_spin_ = std::thread(  spin );
          
           // Listen to the update event. This event is broadcast every simulation iteration.
           impl_->update_connection_ =
           gazebo::event::Events::ConnectWorldUpdateBegin(  
           boost::bind(  
           &GazeboRosControlPrivate::Update,  
           impl_.get(   ) ) );
          
           RCLCPP_INFO(  impl_->model_nh_->get_logger(   ),   "Loaded gazebo_ros2_control." );
          }
          
          // Called by the world update start event
     368  void GazeboRosControlPrivate::Update(   )
          {
           // Get the simulation time and period
           gazebo::common::Time gz_time_now = parent_model_->GetWorld(   )->SimTime(   );
           rclcpp::Time sim_time_ros(  gz_time_now.sec,   gz_time_now.nsec,   RCL_ROS_TIME );
           rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
          
           if (  sim_period >= control_period_ ) {
           controller_manager_->read(  sim_time_ros,   sim_period );
           controller_manager_->update(  sim_time_ros,   sim_period );
           last_update_sim_time_ros_ = sim_time_ros;
           }
          
           // Always set commands on joints,   otherwise at low control frequencies the joints tremble
           // as they are updated at a fraction of gazebo sim time
           // use same time as for read and update call - this is how it is done is ros2_control_node
           controller_manager_->write(  sim_time_ros,   sim_period );
          }
          
          // Called on world reset
     388  void GazeboRosControlPrivate::Reset(   )
          {
           // Reset timing variables to not pass negative update periods to controllers on world reset
           last_update_sim_time_ros_ = rclcpp::Time(  (  int64_t )0,   RCL_ROS_TIME );
          }
          
          // Get the URDF XML from the parameter server
     395  std::string GazeboRosControlPrivate::getURDF(  std::string param_name ) const
          {
           std::string urdf_string;
          
           using namespace std::chrono_literals;
           auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           model_nh_,   robot_description_node_ );
           while (  !parameters_client->wait_for_service(  0.5s ) ) {
           if (  !rclcpp::ok(   ) ) {
           RCLCPP_ERROR(  
           model_nh_->get_logger(   ),   "Interrupted while waiting for %s service. Exiting.",  
           robot_description_node_.c_str(   ) );
           return 0;
           }
           RCLCPP_ERROR(  
           model_nh_->get_logger(   ),   "%s service not available,   waiting again...",  
           robot_description_node_.c_str(   ) );
           }
          
           RCLCPP_INFO(  
           model_nh_->get_logger(   ),   "connected to service!! %s",   robot_description_node_.c_str(   ) );
          
           // search and wait for robot_description on param server
           while (  urdf_string.empty(   ) ) {
           std::string search_param_name;
           RCLCPP_DEBUG(  model_nh_->get_logger(   ),   "param_name %s",   param_name.c_str(   ) );
          
           try {
           auto f = parameters_client->get_parameters(  {param_name} );
           f.wait(   );
           std::vector<rclcpp::Parameter> values = f.get(   );
           urdf_string = values[0].as_string(   );
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  model_nh_->get_logger(   ),   "%s",   e.what(   ) );
           }
          
           if (  !urdf_string.empty(   ) ) {
           break;
           } else {
           RCLCPP_ERROR(  
           model_nh_->get_logger(   ),   "gazebo_ros2_control plugin is waiting for model"
           " URDF in parameter [%s] on the ROS param server.",   search_param_name.c_str(   ) );
           }
           usleep(  100000 );
           }
           RCLCPP_INFO(  
           model_nh_->get_logger(   ),   "Recieved urdf from param server,   parsing..." );
          
           return urdf_string;
          }
          
          // Register this plugin with the simulator
     447  GZ_REGISTER_MODEL_PLUGIN(  GazeboRosControlPlugin )
          } // namespace gazebo_ros2_control

gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp

       1  // Copyright 2021 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <limits>
          #include <map>
          #include <memory>
          #include <string>
          #include <vector>
          #include <utility>
          
          #include "gazebo_ros2_control/gazebo_system.hpp"
          #include "gazebo/sensors/ImuSensor.hh"
          #include "gazebo/sensors/ForceTorqueSensor.hh"
          #include "gazebo/sensors/SensorManager.hh"
          
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          struct MimicJoint
          {
           std::size_t joint_index;
           std::size_t mimicked_joint_index;
           double multiplier = 1.0;
          };
          
      37  class gazebo_ros2_control::GazeboSystemPrivate
          {
          public:
           GazeboSystemPrivate(   ) = default;
          
           ~GazeboSystemPrivate(   ) = default;
          
           /// \brief Degrees od freedom.
           size_t n_dof_;
          
           /// \brief Number of sensors.
           size_t n_sensors_;
          
           /// \brief Gazebo Model Ptr.
           gazebo::physics::ModelPtr parent_model_;
          
           /// \brief last time the write method was called.
           rclcpp::Time last_update_sim_time_ros_;
          
           /// \brief vector with the joint's names.
           std::vector<std::string> joint_names_;
          
           /// \brief vector with the control method defined in the URDF for each joint.
           std::vector<GazeboSystemInterface::ControlMethod> joint_control_methods_;
          
           /// \brief handles to the joints from within Gazebo
           std::vector<gazebo::physics::JointPtr> sim_joints_;
          
           /// \brief vector with the current joint position
           std::vector<double> joint_position_;
          
           /// \brief vector with the current joint velocity
           std::vector<double> joint_velocity_;
          
           /// \brief vector with the current joint effort
           std::vector<double> joint_effort_;
          
           /// \brief vector with the current cmd joint position
           std::vector<double> joint_position_cmd_;
          
           /// \brief vector with the current cmd joint velocity
           std::vector<double> joint_velocity_cmd_;
          
           /// \brief vector with the current cmd joint effort
           std::vector<double> joint_effort_cmd_;
          
           /// \brief handles to the imus from within Gazebo
           std::vector<gazebo::sensors::ImuSensorPtr> sim_imu_sensors_;
          
           /// \brief An array per IMU with 4 orientation,   3 angular velocity and 3 linear acceleration
           std::vector<std::array<double,   10>> imu_sensor_data_;
          
           /// \brief handles to the FT sensors from within Gazebo
           std::vector<gazebo::sensors::ForceTorqueSensorPtr> sim_ft_sensors_;
          
           /// \brief An array per FT sensor for 3D force and torquee
           std::vector<std::array<double,   6>> ft_sensor_data_;
          
           /// \brief state interfaces that will be exported to the Resource Manager
           std::vector<hardware_interface::StateInterface> state_interfaces_;
          
           /// \brief command interfaces that will be exported to the Resource Manager
           std::vector<hardware_interface::CommandInterface> command_interfaces_;
          
           /// \brief mapping of mimicked joints to index of joint they mimic
           std::vector<MimicJoint> mimic_joints_;
          };
          
          namespace gazebo_ros2_control
          {
          
     108  bool GazeboSystem::initSim(  
     109   rclcpp::Node::SharedPtr & model_nh,  
     110   gazebo::physics::ModelPtr parent_model,  
           const hardware_interface::HardwareInfo & hardware_info,  
     112   sdf::ElementPtr sdf )
          {
           this->dataPtr = std::make_unique<GazeboSystemPrivate>(   );
           this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(   );
          
           this->nh_ = model_nh;
           this->dataPtr->parent_model_ = parent_model;
          
           gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world(   )->Physics(   );
          
           std::string physics_type_ = physics->GetType(   );
           if (  physics_type_.empty(   ) ) {
           RCLCPP_ERROR(  this->nh_->get_logger(   ),   "No physics engine configured in Gazebo." );
           return false;
           }
          
           registerJoints(  hardware_info,   parent_model );
           registerSensors(  hardware_info,   parent_model );
          
           if (  this->dataPtr->n_dof_ == 0 && this->dataPtr->n_sensors_ == 0 ) {
           RCLCPP_WARN_STREAM(  this->nh_->get_logger(   ),   "There is no joint or sensor available" );
           return false;
           }
          
           return true;
          }
          
     139  void GazeboSystem::registerJoints(  
           const hardware_interface::HardwareInfo & hardware_info,  
     141   gazebo::physics::ModelPtr parent_model )
          {
           this->dataPtr->n_dof_ = hardware_info.joints.size(   );
          
           this->dataPtr->joint_names_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_control_methods_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_position_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_velocity_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_effort_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_position_cmd_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_velocity_cmd_.resize(  this->dataPtr->n_dof_ );
           this->dataPtr->joint_effort_cmd_.resize(  this->dataPtr->n_dof_ );
          
           for (  unsigned int j = 0; j < this->dataPtr->n_dof_; j++ ) {
           auto & joint_info = hardware_info.joints[j];
           std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
          
           gazebo::physics::JointPtr simjoint = parent_model->GetJoint(  joint_name );
           this->dataPtr->sim_joints_.push_back(  simjoint );
           if (  !simjoint ) {
           RCLCPP_WARN_STREAM(  
           this->nh_->get_logger(   ),   "Skipping joint in the URDF named '" << joint_name <<
           "' which is not in the gazebo model." );
           continue;
           }
          
           // Accept this joint and continue configuration
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "Loading joint: " << joint_name );
          
           std::string suffix = "";
          
           // check if joint is mimicked
           if (  joint_info.parameters.find(  "mimic" ) != joint_info.parameters.end(   ) ) {
           const auto mimicked_joint = joint_info.parameters.at(  "mimic" );
           const auto mimicked_joint_it = std::find_if(  
           hardware_info.joints.begin(   ),   hardware_info.joints.end(   ),  
           [&mimicked_joint](  const hardware_interface::ComponentInfo & info ) {
           return info.name == mimicked_joint;
           } );
           if (  mimicked_joint_it == hardware_info.joints.end(   ) ) {
           throw std::runtime_error(  
           std::string(  "Mimicked joint '" ) + mimicked_joint + "' not found" );
           }
           MimicJoint mimic_joint;
           mimic_joint.joint_index = j;
           mimic_joint.mimicked_joint_index = std::distance(  
           hardware_info.joints.begin(   ),   mimicked_joint_it );
           auto param_it = joint_info.parameters.find(  "multiplier" );
           if (  param_it != joint_info.parameters.end(   ) ) {
           mimic_joint.multiplier = std::stod(  joint_info.parameters.at(  "multiplier" ) );
           } else {
           mimic_joint.multiplier = 1.0;
           }
           RCLCPP_INFO_STREAM(  
           this->nh_->get_logger(   ),  
           "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: "
           << mimic_joint.multiplier );
           this->dataPtr->mimic_joints_.push_back(  mimic_joint );
           suffix = "_mimic";
           }
          
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\tState:" );
          
           auto get_initial_value = [this](  const hardware_interface::InterfaceInfo & interface_info ) {
           if (  !interface_info.initial_value.empty(   ) ) {
           double value = std::stod(  interface_info.initial_value );
           RCLCPP_INFO(  this->nh_->get_logger(   ),   "\t\t\t found initial value: %f",   value );
           return value;
           } else {
           return 0.0;
           }
           };
          
           double initial_position = std::numeric_limits<double>::quiet_NaN(   );
           double initial_velocity = std::numeric_limits<double>::quiet_NaN(   );
           double initial_effort = std::numeric_limits<double>::quiet_NaN(   );
          
           // register the state handles
           for (  unsigned int i = 0; i < joint_info.state_interfaces.size(   ); i++ ) {
           if (  joint_info.state_interfaces[i].name == "position" ) {
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t position" );
           this->dataPtr->state_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_POSITION,  
           &this->dataPtr->joint_position_[j] );
           initial_position = get_initial_value(  joint_info.state_interfaces[i] );
           this->dataPtr->joint_position_[j] = initial_position;
           }
           if (  joint_info.state_interfaces[i].name == "velocity" ) {
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t velocity" );
           this->dataPtr->state_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_VELOCITY,  
           &this->dataPtr->joint_velocity_[j] );
           initial_velocity = get_initial_value(  joint_info.state_interfaces[i] );
           this->dataPtr->joint_velocity_[j] = initial_velocity;
           }
           if (  joint_info.state_interfaces[i].name == "effort" ) {
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t effort" );
           this->dataPtr->state_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_EFFORT,  
           &this->dataPtr->joint_effort_[j] );
           initial_effort = get_initial_value(  joint_info.state_interfaces[i] );
           this->dataPtr->joint_effort_[j] = initial_effort;
           }
           }
          
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\tCommand:" );
          
           // register the command handles
           for (  unsigned int i = 0; i < joint_info.command_interfaces.size(   ); i++ ) {
           if (  joint_info.command_interfaces[i].name == "position" ) {
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t position" );
           this->dataPtr->joint_control_methods_[j] |= POSITION;
           this->dataPtr->command_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_POSITION,  
           &this->dataPtr->joint_position_cmd_[j] );
           if (  !std::isnan(  initial_position ) ) {
           this->dataPtr->joint_position_cmd_[j] = initial_position;
           }
           }
           if (  joint_info.command_interfaces[i].name == "velocity" ) {
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t velocity" );
           this->dataPtr->joint_control_methods_[j] |= VELOCITY;
           this->dataPtr->command_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_VELOCITY,  
           &this->dataPtr->joint_velocity_cmd_[j] );
           if (  !std::isnan(  initial_velocity ) ) {
           this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
           }
           }
           if (  joint_info.command_interfaces[i].name == "effort" ) {
           this->dataPtr->joint_control_methods_[j] |= EFFORT;
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t effort" );
           this->dataPtr->command_interfaces_.emplace_back(  
           joint_name + suffix,  
           hardware_interface::HW_IF_EFFORT,  
           &this->dataPtr->joint_effort_cmd_[j] );
           if (  !std::isnan(  initial_effort ) ) {
           this->dataPtr->joint_effort_cmd_[j] = initial_effort;
           }
           }
           }
           }
          }
          
     290  void GazeboSystem::registerSensors(  
           const hardware_interface::HardwareInfo & hardware_info,  
     292   gazebo::physics::ModelPtr parent_model )
          {
           // Collect gazebo sensor handles
           size_t n_sensors = hardware_info.sensors.size(   );
           std::vector<hardware_interface::ComponentInfo> imu_components_;
           std::vector<hardware_interface::ComponentInfo> ft_sensor_components_;
          
           // This is split in two steps: Count the number and type of sensor and associate the interfaces
           // So we have resize only once the structures where the data will be stored,   and we can safely
           // use pointers to the structures
           for (  unsigned int j = 0; j < n_sensors; j++ ) {
           hardware_interface::ComponentInfo component = hardware_info.sensors[j];
           std::string sensor_name = component.name;
          
           // This can't be used,   because it only looks for sensor in links,   but force_torque_sensor
           // must be in a joint,   as in not found by SensorScopedName
           // std::vector<std::string> gz_sensor_names = parent_model->SensorScopedName(  sensor_name );
          
           // Workaround to find sensors whose parent is a link or joint of parent_model
           std::vector<std::string> gz_sensor_names;
           for (  const auto & s : gazebo::sensors::SensorManager::Instance(   )->GetSensors(   ) ) {
           const std::string sensor_parent = s->ParentName(   );
           if (  s->Name(   ) != sensor_name ) {
           continue;
           }
           if (  (  parent_model->GetJoint(  sensor_parent ) != nullptr ) ||
           parent_model->GetLink(  sensor_parent ) != nullptr )
           {
           gz_sensor_names.push_back(  s->ScopedName(   ) );
           }
           }
           if (  gz_sensor_names.empty(   ) ) {
           RCLCPP_WARN_STREAM(  
           this->nh_->get_logger(   ),   "Skipping sensor in the URDF named '" << sensor_name <<
           "' which is not in the gazebo model." );
           continue;
           }
           if (  gz_sensor_names.size(   ) > 1 ) {
           RCLCPP_WARN_STREAM(  
           this->nh_->get_logger(   ),   "Sensor in the URDF named '" << sensor_name <<
           "' has more than one gazebo sensor with the " <<
           "same name,   only using the first. It has " << gz_sensor_names.size(   ) << " sensors" );
           }
          
           gazebo::sensors::SensorPtr simsensor = gazebo::sensors::SensorManager::Instance(   )->GetSensor(  
           gz_sensor_names[0] );
           if (  !simsensor ) {
           RCLCPP_ERROR_STREAM(  
           this->nh_->get_logger(   ),  
           "Error retrieving sensor '" << sensor_name << " from the sensor manager" );
           continue;
           }
           if (  simsensor->Type(   ) == "imu" ) {
           gazebo::sensors::ImuSensorPtr imu_sensor =
           std::dynamic_pointer_cast<gazebo::sensors::ImuSensor>(  simsensor );
           if (  !imu_sensor ) {
           RCLCPP_ERROR_STREAM(  
           this->nh_->get_logger(   ),  
           "Error retrieving casting sensor '" << sensor_name << " to ImuSensor" );
           continue;
           }
           imu_components_.push_back(  component );
           this->dataPtr->sim_imu_sensors_.push_back(  imu_sensor );
           } else if (  simsensor->Type(   ) == "force_torque" ) {
           gazebo::sensors::ForceTorqueSensorPtr ft_sensor =
           std::dynamic_pointer_cast<gazebo::sensors::ForceTorqueSensor>(  simsensor );
           if (  !ft_sensor ) {
           RCLCPP_ERROR_STREAM(  
           this->nh_->get_logger(   ),  
           "Error retrieving casting sensor '" << sensor_name << " to ForceTorqueSensor" );
           continue;
           }
           ft_sensor_components_.push_back(  component );
           this->dataPtr->sim_ft_sensors_.push_back(  ft_sensor );
           }
           }
          
           this->dataPtr->imu_sensor_data_.resize(  this->dataPtr->sim_imu_sensors_.size(   ) );
           this->dataPtr->ft_sensor_data_.resize(  this->dataPtr->sim_ft_sensors_.size(   ) );
           this->dataPtr->n_sensors_ = this->dataPtr->sim_imu_sensors_.size(   ) +
           this->dataPtr->sim_ft_sensors_.size(   );
          
           for (  unsigned int i = 0; i < imu_components_.size(   ); i++ ) {
           const std::string & sensor_name = imu_components_[i].name;
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "Loading sensor: " << sensor_name );
           RCLCPP_INFO_STREAM(  
           this->nh_->get_logger(   ),   "\tState:" );
           for (  const auto & state_interface : imu_components_[i].state_interfaces ) {
           static const std::map<std::string,   size_t> interface_name_map = {
           {"orientation.x",   0},  
           {"orientation.y",   1},  
           {"orientation.z",   2},  
           {"orientation.w",   3},  
           {"angular_velocity.x",   4},  
           {"angular_velocity.y",   5},  
           {"angular_velocity.z",   6},  
           {"linear_acceleration.x",   7},  
           {"linear_acceleration.y",   8},  
           {"linear_acceleration.z",   9},  
           };
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t " << state_interface.name );
          
           size_t data_index = interface_name_map.at(  state_interface.name );
           this->dataPtr->state_interfaces_.emplace_back(  
           sensor_name,  
           state_interface.name,  
           &this->dataPtr->imu_sensor_data_[i][data_index] );
           }
           }
           for (  unsigned int i = 0; i < ft_sensor_components_.size(   ); i++ ) {
           const std::string & sensor_name = ft_sensor_components_[i].name;
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "Loading sensor: " << sensor_name );
           RCLCPP_INFO_STREAM(  
           this->nh_->get_logger(   ),   "\tState:" );
           for (  const auto & state_interface : ft_sensor_components_[i].state_interfaces ) {
           static const std::map<std::string,   size_t> interface_name_map = {
           {"force.x",   0},  
           {"force.y",   1},  
           {"force.z",   2},  
           {"torque.x",   3},  
           {"torque.y",   4},  
           {"torque.z",   5}
           };
           RCLCPP_INFO_STREAM(  this->nh_->get_logger(   ),   "\t\t " << state_interface.name );
          
           size_t data_index = interface_name_map.at(  state_interface.name );
           this->dataPtr->state_interfaces_.emplace_back(  
           sensor_name,  
           state_interface.name,  
           &this->dataPtr->ft_sensor_data_[i][data_index] );
           }
           }
          }
          
          CallbackReturn
     427  GazeboSystem::on_init(  const hardware_interface::HardwareInfo & system_info )
          {
           if (  hardware_interface::SystemInterface::on_init(  system_info ) != CallbackReturn::SUCCESS ) {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::SUCCESS;
          }
          
          std::vector<hardware_interface::StateInterface>
     436  GazeboSystem::export_state_interfaces(   )
          {
           return std::move(  this->dataPtr->state_interfaces_ );
          }
          
          std::vector<hardware_interface::CommandInterface>
     442  GazeboSystem::export_command_interfaces(   )
          {
           return std::move(  this->dataPtr->command_interfaces_ );
          }
          
     447  CallbackReturn GazeboSystem::on_activate(  const rclcpp_lifecycle::State & previous_state )
          {
           return CallbackReturn::SUCCESS;
          }
          
     452  CallbackReturn GazeboSystem::on_deactivate(  const rclcpp_lifecycle::State & previous_state )
          {
           return CallbackReturn::SUCCESS;
          }
          
     457  hardware_interface::return_type GazeboSystem::read(  
     458   const rclcpp::Time & time,  
     459   const rclcpp::Duration & period )
          {
           for (  unsigned int j = 0; j < this->dataPtr->joint_names_.size(   ); j++ ) {
           if (  this->dataPtr->sim_joints_[j] ) {
           this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(  0 );
           this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(  0 );
           this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(  0u );
           }
           }
          
           for (  unsigned int j = 0; j < this->dataPtr->sim_imu_sensors_.size(   ); j++ ) {
           auto sim_imu = this->dataPtr->sim_imu_sensors_[j];
           this->dataPtr->imu_sensor_data_[j][0] = sim_imu->Orientation(   ).X(   );
           this->dataPtr->imu_sensor_data_[j][1] = sim_imu->Orientation(   ).Y(   );
           this->dataPtr->imu_sensor_data_[j][2] = sim_imu->Orientation(   ).Z(   );
           this->dataPtr->imu_sensor_data_[j][3] = sim_imu->Orientation(   ).W(   );
          
           this->dataPtr->imu_sensor_data_[j][4] = sim_imu->AngularVelocity(   ).X(   );
           this->dataPtr->imu_sensor_data_[j][5] = sim_imu->AngularVelocity(   ).Y(   );
           this->dataPtr->imu_sensor_data_[j][6] = sim_imu->AngularVelocity(   ).Z(   );
          
           this->dataPtr->imu_sensor_data_[j][7] = sim_imu->LinearAcceleration(   ).X(   );
           this->dataPtr->imu_sensor_data_[j][8] = sim_imu->LinearAcceleration(   ).Y(   );
           this->dataPtr->imu_sensor_data_[j][9] = sim_imu->LinearAcceleration(   ).Z(   );
           }
          
           for (  unsigned int j = 0; j < this->dataPtr->sim_ft_sensors_.size(   ); j++ ) {
           auto sim_ft_sensor = this->dataPtr->sim_ft_sensors_[j];
           this->dataPtr->imu_sensor_data_[j][0] = sim_ft_sensor->Force(   ).X(   );
           this->dataPtr->imu_sensor_data_[j][1] = sim_ft_sensor->Force(   ).Y(   );
           this->dataPtr->imu_sensor_data_[j][2] = sim_ft_sensor->Force(   ).Z(   );
          
           this->dataPtr->imu_sensor_data_[j][3] = sim_ft_sensor->Torque(   ).X(   );
           this->dataPtr->imu_sensor_data_[j][4] = sim_ft_sensor->Torque(   ).Y(   );
           this->dataPtr->imu_sensor_data_[j][5] = sim_ft_sensor->Torque(   ).Z(   );
           }
           return hardware_interface::return_type::OK;
          }
          
     498  hardware_interface::return_type GazeboSystem::write(  
     499   const rclcpp::Time & time,  
     500   const rclcpp::Duration & period )
          {
           // Get the simulation time and period
           gazebo::common::Time gz_time_now = this->dataPtr->parent_model_->GetWorld(   )->SimTime(   );
           rclcpp::Time sim_time_ros(  gz_time_now.sec,   gz_time_now.nsec );
           rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
          
           // set values of all mimic joints with respect to mimicked joint
           for (  const auto & mimic_joint : this->dataPtr->mimic_joints_ ) {
           if (  this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
           this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION )
           {
           this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
           mimic_joint.multiplier *
           this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
           }
           if (  this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY &&
           this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY )
           {
           this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
           mimic_joint.multiplier *
           this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
           }
           if (  this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT &&
           this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT )
           {
           this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
           mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
           }
           }
          
           for (  unsigned int j = 0; j < this->dataPtr->joint_names_.size(   ); j++ ) {
           if (  this->dataPtr->sim_joints_[j] ) {
           if (  this->dataPtr->joint_control_methods_[j] & POSITION ) {
           this->dataPtr->sim_joints_[j]->SetPosition(  
           0,   this->dataPtr->joint_position_cmd_[j],  
           true );
           }
           if (  this->dataPtr->joint_control_methods_[j] & VELOCITY ) {
           this->dataPtr->sim_joints_[j]->SetVelocity(  
           0,  
           this->dataPtr->joint_velocity_cmd_[j] );
           }
           if (  this->dataPtr->joint_control_methods_[j] & EFFORT ) {
           const double effort = this->dataPtr->joint_effort_cmd_[j];
           this->dataPtr->sim_joints_[j]->SetForce(  0,   effort );
           }
           }
           }
          
           this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
          
           return hardware_interface::return_type::OK;
          }
          } // namespace gazebo_ros2_control
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
     557  PLUGINLIB_EXPORT_CLASS(  
           gazebo_ros2_control::GazeboSystem,   gazebo_ros2_control::GazeboSystemInterface )

gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_diff_drive.cpp

       1  // Copyright 2022 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include <rclcpp/rclcpp.hpp>
          
          #include <geometry_msgs/msg/twist.hpp>
          
          using namespace std::chrono_literals;
          
      23  int main(  int argc,   char * argv[] )
          {
           rclcpp::init(  argc,   argv );
          
           std::shared_ptr<rclcpp::Node> node =
           std::make_shared<rclcpp::Node>(  "diff_drive_test_node" );
          
           auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(  
           "/diff_drive_base_controller/cmd_vel_unstamped",   10 );
          
           RCLCPP_INFO(  node->get_logger(   ),   "node created" );
          
           geometry_msgs::msg::Twist command;
          
           command.linear.x = 0.2;
           command.linear.y = 0.0;
           command.linear.z = 0.0;
          
           command.angular.x = 0.0;
           command.angular.y = 0.0;
           command.angular.z = 0.1;
          
           while (  1 ) {
           publisher->publish(  command );
           std::this_thread::sleep_for(  50ms );
           rclcpp::spin_some(  node );
           }
           rclcpp::shutdown(   );
          
           return 0;
          }

gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_effort.cpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "std_msgs/msg/float64_multi_array.hpp"
          
          std::shared_ptr<rclcpp::Node> node;
          
      25  int main(  int argc,   char * argv[] )
          {
           rclcpp::init(  argc,   argv );
          
           node = std::make_shared<rclcpp::Node>(  "effort_test_node" );
          
           auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(  
           "/effort_controllers/commands",   10 );
          
           RCLCPP_INFO(  node->get_logger(   ),   "node created" );
          
           std_msgs::msg::Float64MultiArray commands;
          
           using namespace std::chrono_literals;
          
           commands.data.push_back(  0 );
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 100;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = -200;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 0;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
           rclcpp::shutdown(   );
          
           return 0;
          }

gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_gripper.cpp

       1  // Copyright 2022 Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "std_msgs/msg/float64_multi_array.hpp"
          
          std::shared_ptr<rclcpp::Node> node;
          
      25  int main(  int argc,   char * argv[] )
          {
           rclcpp::init(  argc,   argv );
          
           node = std::make_shared<rclcpp::Node>(  "gripper_test_node" );
          
           auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(  
           "/gripper_controller/commands",   10 );
          
           RCLCPP_INFO(  node->get_logger(   ),   "node created" );
          
           std_msgs::msg::Float64MultiArray commands;
          
           using namespace std::chrono_literals;
          
           commands.data.push_back(  0 );
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 0.38;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 0.19;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 0;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
           rclcpp::shutdown(   );
          
           return 0;
          }

gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_position.cpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          
          #include "control_msgs/action/follow_joint_trajectory.hpp"
          
          std::shared_ptr<rclcpp::Node> node;
          bool common_goal_accepted = false;
          rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
          int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
          
      29  void common_goal_response(  
           rclcpp_action::ClientGoalHandle
           <control_msgs::action::FollowJointTrajectory>::SharedPtr goal_handle )
          {
           RCLCPP_DEBUG(  
           node->get_logger(   ),   "common_goal_response time: %f",  
           rclcpp::Clock(   ).now(   ).seconds(   ) );
           if (  !goal_handle ) {
           common_goal_accepted = false;
           printf(  "Goal rejected\n" );
           } else {
           common_goal_accepted = true;
           printf(  "Goal accepted\n" );
           }
          }
          
      45  void common_result_response(  
           const rclcpp_action::ClientGoalHandle
           <control_msgs::action::FollowJointTrajectory>::WrappedResult & result )
          {
           printf(  "common_result_response time: %f\n",   rclcpp::Clock(   ).now(   ).seconds(   ) );
           common_resultcode = result.code;
           common_action_result_code = result.result->error_code;
           switch (  result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED:
           printf(  "SUCCEEDED result code\n" );
           break;
           case rclcpp_action::ResultCode::ABORTED:
           printf(  "Goal was aborted\n" );
           return;
           case rclcpp_action::ResultCode::CANCELED:
           printf(  "Goal was canceled\n" );
           return;
           default:
           printf(  "Unknown result code\n" );
           return;
           }
          }
          
      68  void common_feedback(  
           rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,  
           const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback )
          {
           std::cout << "feedback->desired.positions :";
           for (  auto & x : feedback->desired.positions ) {
           std::cout << x << "\t";
           }
           std::cout << std::endl;
           std::cout << "feedback->desired.velocities :";
           for (  auto & x : feedback->desired.velocities ) {
           std::cout << x << "\t";
           }
           std::cout << std::endl;
          }
          
      84  int main(  int argc,   char * argv[] )
          {
           rclcpp::init(  argc,   argv );
          
           node = std::make_shared<rclcpp::Node>(  "trajectory_test_node" );
          
           std::cout << "node created" << std::endl;
          
           rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
           action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(  
           node->get_node_base_interface(   ),  
           node->get_node_graph_interface(   ),  
           node->get_node_logging_interface(   ),  
           node->get_node_waitables_interface(   ),  
           "/joint_trajectory_controller/follow_joint_trajectory" );
          
           bool response =
           action_client->wait_for_action_server(  std::chrono::seconds(  1 ) );
           if (  !response ) {
           throw std::runtime_error(  "could not get action server" );
           }
           std::cout << "Created action server" << std::endl;
          
           std::vector<std::string> joint_names = {"slider_to_cart"};
          
           std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
           trajectory_msgs::msg::JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  0.0 ); // start asap
           point.positions.resize(  joint_names.size(   ) );
          
           point.positions[0] = 0.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint point2;
           point2.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           point2.positions.resize(  joint_names.size(   ) );
           point2.positions[0] = -1.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint point3;
           point3.time_from_start = rclcpp::Duration::from_seconds(  2.0 );
           point3.positions.resize(  joint_names.size(   ) );
           point3.positions[0] = 1.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint point4;
           point4.time_from_start = rclcpp::Duration::from_seconds(  3.0 );
           point4.positions.resize(  joint_names.size(   ) );
           point4.positions[0] = 0.0;
          
           points.push_back(  point );
           points.push_back(  point2 );
           points.push_back(  point3 );
           points.push_back(  point4 );
          
           rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
           opt.goal_response_callback = std::bind(  common_goal_response,   std::placeholders::_1 );
           opt.result_callback = std::bind(  common_result_response,   std::placeholders::_1 );
           opt.feedback_callback = std::bind(  common_feedback,   std::placeholders::_1,   std::placeholders::_2 );
          
           control_msgs::action::FollowJointTrajectory_Goal goal_msg;
           goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(  1.0 );
           goal_msg.trajectory.joint_names = joint_names;
           goal_msg.trajectory.points = points;
          
           auto goal_handle_future = action_client->async_send_goal(  goal_msg,   opt );
          
           if (  rclcpp::spin_until_future_complete(  node,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node->get_logger(   ),   "send goal call failed :(  " );
           return 1;
           }
           RCLCPP_INFO(  node->get_logger(   ),   "send goal call ok : )" );
          
           rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
           goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node->get_logger(   ),   "Goal was rejected by server" );
           return 1;
           }
           RCLCPP_INFO(  node->get_logger(   ),   "Goal was accepted by server" );
          
           // Wait for the server to be done with the goal
           auto result_future = action_client->async_get_result(  goal_handle );
           RCLCPP_INFO(  node->get_logger(   ),   "Waiting for result" );
           if (  rclcpp::spin_until_future_complete(  node,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node->get_logger(   ),   "get result call failed :(  " );
           return 1;
           }
          
           std::cout << "async_send_goal" << std::endl;
           rclcpp::shutdown(   );
          
           return 0;
          }

gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_velocity.cpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "std_msgs/msg/float64_multi_array.hpp"
          
          std::shared_ptr<rclcpp::Node> node;
          
      25  int main(  int argc,   char * argv[] )
          {
           rclcpp::init(  argc,   argv );
          
           node = std::make_shared<rclcpp::Node>(  "velocity_test_node" );
          
           auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(  
           "/velocity_controller/commands",   10 );
          
           RCLCPP_INFO(  node->get_logger(   ),   "node created" );
          
           std_msgs::msg::Float64MultiArray commands;
          
           using namespace std::chrono_literals;
          
           commands.data.push_back(  0 );
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 1;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = -1;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
          
           commands.data[0] = 0;
           publisher->publish(  commands );
           std::this_thread::sleep_for(  1s );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_amcl/include/nav2_amcl/amcl_node.hpp

          /*
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #ifndef NAV2_AMCL__AMCL_NODE_HPP_
          #define NAV2_AMCL__AMCL_NODE_HPP_
          
          #include <atomic>
          #include <map>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "message_filters/subscriber.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_amcl/motion_model/motion_model.hpp"
          #include "nav2_amcl/sensors/laser/laser.hpp"
          #include "nav2_msgs/msg/particle.hpp"
          #include "nav2_msgs/msg/particle_cloud.hpp"
          #include "nav_msgs/srv/set_map.hpp"
          #include "sensor_msgs/msg/laser_scan.hpp"
          #include "std_srvs/srv/empty.hpp"
          #include "tf2_ros/transform_broadcaster.h"
          #include "tf2_ros/transform_listener.h"
          #include "pluginlib/class_loader.hpp"
          
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wunused-parameter"
          #pragma GCC diagnostic ignored "-Wreorder"
          #include "tf2_ros/message_filter.h"
          #pragma GCC diagnostic pop
          
          #define NEW_UNIFORM_SAMPLING 1
          
          namespace nav2_amcl
          {
          /*
           * @class AmclNode
           * @brief ROS wrapper for AMCL
           */
      59  class AmclNode : public nav2_util::LifecycleNode
          {
          public:
           /*
           * @brief AMCL constructor
           * @param options Additional options to control creation of the node.
           */
      66   explicit AmclNode(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /*
           * @brief AMCL destructor
           */
      70   ~AmclNode(   );
          
          protected:
           /*
           * @brief Lifecycle configure
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /*
           * @brief Lifecycle activate
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /*
           * @brief Lifecycle deactivate
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /*
           * @brief Lifecycle cleanup
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /*
           * @brief Lifecycle shutdown
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          
           // Since the sensor data from gazebo or the robot is not lifecycle enabled,   we won't
           // respond until we're in the active state
           std::atomic<bool> active_{false};
          
           // Dedicated callback group and executor for services and subscriptions in AmclNode,  
           // in order to isolate TF timer used in message filter.
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
           std::unique_ptr<nav2_util::NodeThread> executor_thread_;
          
           // Pose hypothesis
           typedef struct
           {
           double weight; // Total weight (  weights sum to 1 )
           pf_vector_t pf_pose_mean; // Mean of pose esimate
           pf_matrix_t pf_pose_cov; // Covariance of pose estimate
           } amcl_hyp_t;
          
           // Map-related
           /*
           * @brief Get new map from ROS topic to localize in
           * @param msg Map message
           */
     127   void mapReceived(  const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
           /*
           * @brief Handle a new map message
           * @param msg Map message
           */
     132   void handleMapMessage(  const nav_msgs::msg::OccupancyGrid & msg );
           /*
           * @brief Creates lookup table of free cells in map
           */
     136   void createFreeSpaceVector(   );
           /*
           * @brief Frees allocated map related memory
           */
     140   void freeMapDependentMemory(   );
     141   map_t * map_{nullptr};
           /*
           * @brief Convert an occupancy grid map to an AMCL map
           * @param map_msg Map message
           * @return pointer to map for AMCL to use
           */
     147   map_t * convertMap(  const nav_msgs::msg::OccupancyGrid & map_msg );
           bool first_map_only_{true};
           std::atomic<bool> first_map_received_{false};
           amcl_hyp_t * initial_pose_hyp_;
           std::recursive_mutex mutex_;
           rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
          #if NEW_UNIFORM_SAMPLING
           static std::vector<std::pair<int,   int>> free_space_indices;
          #endif
          
           // Transforms
           /*
           * @brief Initialize required ROS transformations
           */
           void initTransforms(   );
           std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           bool sent_first_transform_{false};
           bool latest_tf_valid_{false};
           tf2::Transform latest_tf_;
          
           // Message filters
           /*
           * @brief Initialize incoming data message subscribers and filters
           */
           void initMessageFilters(   );
           std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,  
           rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
           std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
           message_filters::Connection laser_scan_connection_;
          
           // Publishers and subscribers
           /*
           * @brief Initialize pub subs of AMCL
           */
           void initPubSub(   );
           rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
           initial_pose_sub_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
           pose_pub_;
           rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
           particle_cloud_pub_;
           /*
           * @brief Handle with an initial pose estimate is received
           */
           void initialPoseReceived(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg );
           /*
           * @brief Handle when a laser scan is received
           */
           void laserReceived(  sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan );
          
           // Services and service callbacks
           /*
           * @brief Initialize state services
           */
           void initServices(   );
           rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
           /*
           * @brief Service callback for a global relocalization request
           */
           void globalLocalizationCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<std_srvs::srv::Empty::Request> request,  
           std::shared_ptr<std_srvs::srv::Empty::Response> response );
          
           // Let amcl update samples without requiring motion
           rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
           /*
           * @brief Request an AMCL update even though the robot hasn't moved
           */
           void nomotionUpdateCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<std_srvs::srv::Empty::Request> request,  
           std::shared_ptr<std_srvs::srv::Empty::Response> response );
          
           // Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs
           std::atomic<bool> force_update_{false};
          
           // Odometry
           /*
           * @brief Initialize odometry
           */
           void initOdometry(   );
           std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
           geometry_msgs::msg::PoseStamped latest_odom_pose_;
           geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
           double init_pose_[3]; // Initial robot pose
           double init_cov_[3];
           pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl",  
           "nav2_amcl::MotionModel"};
           /*
           * @brief Get robot pose in odom frame using TF
           */
           bool getOdomPose(  
           // Helper to get odometric pose from transform system
           geometry_msgs::msg::PoseStamped & pose,  
           double & x,   double & y,   double & yaw,  
           const rclcpp::Time & sensor_timestamp,   const std::string & frame_id );
           std::atomic<bool> first_pose_sent_;
          
           // Particle filter
           /*
           * @brief Initialize particle filter
           */
           void initParticleFilter(   );
           /*
           * @brief Pose-generating function used to uniformly distribute particles over the map
           */
           static pf_vector_t uniformPoseGenerator(  void * arg );
           pf_t * pf_{nullptr};
           bool pf_init_;
           pf_vector_t pf_odom_pose_;
           int resample_count_{0};
          
           // Laser scan related
           /*
           * @brief Initialize laser scan
           */
           void initLaserScan(   );
           /*
           * @brief Create a laser object
           */
           nav2_amcl::Laser * createLaserObject(   );
           int scan_error_count_{0};
           std::vector<nav2_amcl::Laser *> lasers_;
           std::vector<bool> lasers_update_;
           std::map<std::string,   int> frame_to_laser_;
           rclcpp::Time last_laser_received_ts_;
          
           /*
           * @brief Check if sufficient time has elapsed to get an update
           */
           bool checkElapsedTime(  std::chrono::seconds check_interval,   rclcpp::Time last_time );
           rclcpp::Time last_time_printed_msg_;
           /*
           * @brief Add a new laser scanner if a new one is received in the laser scallbacks
           */
           bool addNewScanner(  
           int & laser_index,  
           const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
           const std::string & laser_scan_frame_id,  
           geometry_msgs::msg::PoseStamped & laser_pose );
           /*
           * @brief Whether the pf needs to be updated
           */
           bool shouldUpdateFilter(  const pf_vector_t pose,   pf_vector_t & delta );
           /*
           * @brief Update the PF
           */
           bool updateFilter(  
           const int & laser_index,  
           const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
           const pf_vector_t & pose );
           /*
           * @brief Publish particle cloud
           */
           void publishParticleCloud(  const pf_sample_set_t * set );
           /*
           * @brief Get the current state estimat hypothesis from the particle cloud
           */
           bool getMaxWeightHyp(  
           std::vector<amcl_hyp_t> & hyps,   amcl_hyp_t & max_weight_hyps,  
           int & max_weight_hyp );
           /*
           * @brief Publish robot pose in map frame from AMCL
           */
           void publishAmclPose(  
           const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
           const std::vector<amcl_hyp_t> & hyps,   const int & max_weight_hyp );
           /*
           * @brief Determine TF transformation from map to odom
           */
           void calculateMaptoOdomTransform(  
           const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
           const std::vector<amcl_hyp_t> & hyps,  
           const int & max_weight_hyp );
           /*
           * @brief Publish TF transformation from map to odom
           */
           void sendMapToOdomTransform(  const tf2::TimePoint & transform_expiration );
           /*
           * @brief Handle a new pose estimate callback
           */
           void handleInitialPose(  geometry_msgs::msg::PoseWithCovarianceStamped & msg );
           bool init_pose_received_on_inactive{false};
           bool initial_pose_is_known_{false};
           bool set_initial_pose_{false};
           bool always_reset_initial_pose_;
           double initial_pose_x_;
           double initial_pose_y_;
           double initial_pose_z_;
           double initial_pose_yaw_;
          
           /*
           * @brief Get ROS parameters for node
           */
           void initParameters(   );
           double alpha1_;
           double alpha2_;
           double alpha3_;
           double alpha4_;
           double alpha5_;
           std::string base_frame_id_;
           double beam_skip_distance_;
           double beam_skip_error_threshold_;
           double beam_skip_threshold_;
           bool do_beamskip_;
           std::string global_frame_id_;
           double lambda_short_;
           double laser_likelihood_max_dist_;
           double laser_max_range_;
           double laser_min_range_;
           std::string sensor_model_type_;
           int max_beams_;
           int max_particles_;
           int min_particles_;
           std::string odom_frame_id_;
           double pf_err_;
           double pf_z_;
           double alpha_fast_;
           double alpha_slow_;
           int resample_interval_;
           std::string robot_model_type_;
           tf2::Duration save_pose_period_;
           double sigma_hit_;
           bool tf_broadcast_;
           tf2::Duration transform_tolerance_;
           double a_thresh_;
           double d_thresh_;
           double z_hit_;
           double z_max_;
           double z_short_;
           double z_rand_;
           std::string scan_topic_{"scan"};
           std::string map_topic_{"map"};
          };
          
          } // namespace nav2_amcl
          
          #endif // NAV2_AMCL__AMCL_NODE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/angleutils.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #ifndef NAV2_AMCL__ANGLEUTILS_HPP_
          #define NAV2_AMCL__ANGLEUTILS_HPP_
          
          #include <math.h>
          
          namespace nav2_amcl
          {
          
          /*
           * @class angleutils
           * @brief Some utilities for working with angles
           */
      34  class angleutils
          {
          public:
           /*
           * @brief Normalize angles
           * @brief z Angle to normalize
           * @return normalized angle
           */
      42   static double normalize(  double z );
          
           /*
           * @brief Find minimum distance between 2 angles
           * @brief a Angle 1
           * @brief b Angle 2
           * @return normalized angle difference
           */
      50   static double angle_diff(  double a,   double b );
          };
          
          inline double
      54  angleutils::normalize(  double z )
          {
           return atan2(  sin(  z ),   cos(  z ) );
          }
          
          inline double
      60  angleutils::angle_diff(  double a,   double b )
          {
           a = normalize(  a );
           b = normalize(  b );
           double d1 = a - b;
           double d2 = 2 * M_PI - fabs(  d1 );
           if (  d1 > 0 ) {
           d2 *= -1.0;
           }
           if (  fabs(  d1 ) < fabs(  d2 ) ) {
           return d1;
           } else {
           return d2;
           }
          }
          
          } // namespace nav2_amcl
          
          #endif // NAV2_AMCL__ANGLEUTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/map/map.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Global map (  grid-based )
           * Author: Andrew Howard
           * Date: 6 Feb 2003
           * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
           **************************************************************************/
          
          #ifndef NAV2_AMCL__MAP__MAP_HPP_
          #define NAV2_AMCL__MAP__MAP_HPP_
          
          #include <stdint.h>
          
          #ifdef __cplusplus
          extern "C" {
          #endif
          
          // Forward declarations
          struct _rtk_fig_t;
          
          
          // Limits
          #define MAP_WIFI_MAX_LEVELS 8
          
          
          // Description for a single map cell.
          typedef struct
          {
           // Occupancy state (  -1 = free,   0 = unknown,   +1 = occ )
           int occ_state;
          
           // Distance to the nearest occupied cell
           double occ_dist;
          
           // Wifi levels
           // int wifi_levels[MAP_WIFI_MAX_LEVELS];
          } map_cell_t;
          
          
          // Description for a map
          typedef struct
          {
           // Map origin; the map is a viewport onto a conceptual larger map.
           double origin_x,   origin_y;
          
           // Map scale (  m/cell )
           double scale;
          
           // Map dimensions (  number of cells )
           int size_x,   size_y;
          
           // The map data,   stored as a grid
           map_cell_t * cells;
          
           // Max distance at which we care about obstacles,   for constructing
           // likelihood field
           double max_occ_dist;
          } map_t;
          
          
          /**************************************************************************
           * Basic map functions
           **************************************************************************/
          
          // Create a new (  empty ) map
      85  map_t * map_alloc(  void );
          
          // Destroy a map
      88  void map_free(  map_t * map );
          
          // Update the cspace distances
      91  void map_update_cspace(  map_t * map,   double max_occ_dist );
          
          
          /**************************************************************************
           * Range functions
           **************************************************************************/
          
          // Extract a single range reading from the map
      99  double map_calc_range(  map_t * map,   double ox,   double oy,   double oa,   double max_range );
          
          
          /**************************************************************************
           * GUI/diagnostic functions
           **************************************************************************/
          
          // Draw the occupancy grid
     107  void map_draw_occ(  map_t * map,   struct _rtk_fig_t * fig );
          
          // Draw the cspace map
     110  void map_draw_cspace(  map_t * map,   struct _rtk_fig_t * fig );
          
          // Draw a wifi map
     113  void map_draw_wifi(  map_t * map,   struct _rtk_fig_t * fig,   int index );
          
          
          /**************************************************************************
           * Map manipulation macros
           **************************************************************************/
          
          // Convert from map index to world coords
          #define MAP_WXGX(  map,   i ) (  map->origin_x + (  (  i ) - map->size_x / 2 ) * map->scale )
          #define MAP_WYGY(  map,   j ) (  map->origin_y + (  (  j ) - map->size_y / 2 ) * map->scale )
          
          // Convert from world coords to map coords
          #define MAP_GXWX(  map,   x ) (  floor(  (  x - map->origin_x ) / map->scale + 0.5 ) + map->size_x / 2 )
          #define MAP_GYWY(  map,   y ) (  floor(  (  y - map->origin_y ) / map->scale + 0.5 ) + map->size_y / 2 )
          
          // Test to see if the given map coords lie within the absolute map bounds.
          #define MAP_VALID(  map,   i,   j ) (  (  i >= 0 ) && (  i < map->size_x ) && (  j >= 0 ) && (  j < map->size_y ) )
          
          // Compute the cell index for the given map coords.
          #define MAP_INDEX(  map,   i,   j ) (  (  i ) + (  j ) * map->size_x )
          
          #ifdef __cplusplus
          }
          #endif
          
          #endif // NAV2_AMCL__MAP__MAP_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
          #define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
          
          #include <sys/types.h>
          #include <math.h>
          #include <algorithm>
          #include "nav2_amcl/motion_model/motion_model.hpp"
          #include "nav2_amcl/angleutils.hpp"
          
          
          namespace nav2_amcl
          {
          
      35  class DifferentialMotionModel : public nav2_amcl::MotionModel
          {
          public:
      38   virtual void initialize(  
           double alpha1,   double alpha2,   double alpha3,   double alpha4,  
           double alpha5 );
      41   virtual void odometryUpdate(  pf_t * pf,   const pf_vector_t & pose,   const pf_vector_t & delta );
          
          private:
           double alpha1_,   alpha2_,   alpha3_,   alpha4_,   alpha5_;
          };
          } // namespace nav2_amcl
          #endif // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // This library is free software; you can redistribute it and/or
          // modify it under the terms of the GNU Lesser General Public
          // License as published by the Free Software Foundation; either
          // version 2.1 of the License,   or (  at your option ) any later version.
          //
          // This library is distributed in the hope that it will be useful,  
          // but WITHOUT ANY WARRANTY; without even the implied warranty of
          // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
          // Lesser General Public License for more details.
          //
          // You should have received a copy of the GNU Lesser General Public
          // License along with this library; if not,   write to the Free Software
          // Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
          
          #ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
          #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
          
          #include <string>
          #include <memory>
          
          #include "nav2_amcl/pf/pf.hpp"
          #include "nav2_amcl/pf/pf_pdf.hpp"
          
          namespace nav2_amcl
          {
          
          /**
           * @class nav2_amcl::MotionModel
           * @brief An abstract motion model class
           */
      33  class MotionModel
          {
          public:
           virtual ~MotionModel(   ) = default;
          
           /**
           * @brief An factory to create motion models
           * @param type Type of motion model to create in factory
           * @param alpha1 error parameters,   see documentation
           * @param alpha2 error parameters,   see documentation
           * @param alpha3 error parameters,   see documentation
           * @param alpha4 error parameters,   see documentation
           * @param alpha5 error parameters,   see documentation
           * @return MotionModel A pointer to the motion model it created
           */
           virtual void initialize(  
           double alpha1,   double alpha2,   double alpha3,   double alpha4,  
           double alpha5 ) = 0;
          
           /**
           * @brief Update on new odometry data
           * @param pf The particle filter to update
           * @param pose pose of robot in odometry update
           * @param delta change in pose in odometry update
           */
           virtual void odometryUpdate(  pf_t * pf,   const pf_vector_t & pose,   const pf_vector_t & delta ) = 0;
          };
          } // namespace nav2_amcl
          
          #endif // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
          #define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
          
          #include <sys/types.h>
          #include <math.h>
          #include <algorithm>
          #include "nav2_amcl/motion_model/motion_model.hpp"
          #include "nav2_amcl/angleutils.hpp"
          
          
          namespace nav2_amcl
          {
          
      35  class OmniMotionModel : public nav2_amcl::MotionModel
          {
          public:
      38   virtual void initialize(  
           double alpha1,   double alpha2,   double alpha3,   double alpha4,  
           double alpha5 );
      41   virtual void odometryUpdate(  pf_t * pf,   const pf_vector_t & pose,   const pf_vector_t & delta );
          
          private:
           double alpha1_,   alpha2_,   alpha3_,   alpha4_,   alpha5_;
          };
          } // namespace nav2_amcl
          #endif // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/eig3.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /* Eigen-decomposition for symmetric 3x3 real matrices.
           Public domain,   copied from the public domain Java library JAMA. */
          
          #ifndef NAV2_AMCL__PF__EIG3_HPP_
          #define NAV2_AMCL__PF__EIG3_HPP_
          
          /* Symmetric matrix A => eigenvectors in columns of V,   corresponding
           eigenvalues in d. */
      29  void eigen_decomposition(  double A[3][3],   double V[3][3],   double d[3] );
          
          #endif // NAV2_AMCL__PF__EIG3_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Simple particle filter for localization.
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
           *************************************************************************/
          
          #ifndef NAV2_AMCL__PF__PF_HPP_
          #define NAV2_AMCL__PF__PF_HPP_
          
          #include "nav2_amcl/pf/pf_vector.hpp"
          #include "nav2_amcl/pf/pf_kdtree.hpp"
          
          #ifdef __cplusplus
          extern "C" {
          #endif
          
          // Forward declarations
          struct _pf_t;
          struct _rtk_fig_t;
          struct _pf_sample_set_t;
          
          // Function prototype for the initialization model; generates a sample pose from
          // an appropriate distribution.
          typedef pf_vector_t (  * pf_init_model_fn_t ) (  void * init_data );
          
          // Function prototype for the action model; generates a sample pose from
          // an appropriate distribution
          typedef void (  * pf_action_model_fn_t ) (  
           void * action_data,  
           struct _pf_sample_set_t * set );
          
          // Function prototype for the sensor model; determines the probability
          // for the given set of sample poses.
          typedef double (  * pf_sensor_model_fn_t ) (  
           void * sensor_data,  
           struct _pf_sample_set_t * set );
          
          
          // Information for a single sample
          typedef struct
          {
           // Pose represented by this sample
           pf_vector_t pose;
          
           // Weight for this pose
           double weight;
          } pf_sample_t;
          
          
          // Information for a cluster of samples
          typedef struct
          {
           // Number of samples
           int count;
          
           // Total weight of samples in this cluster
           double weight;
          
           // Cluster statistics
           pf_vector_t mean;
           pf_matrix_t cov;
          
           // Workspace
           double m[4],   c[2][2];
          } pf_cluster_t;
          
          
          // Information for a set of samples
          typedef struct _pf_sample_set_t
          {
           // The samples
           int sample_count;
           pf_sample_t * samples;
          
           // A kdtree encoding the histogram
           pf_kdtree_t * kdtree;
          
           // Clusters
           int cluster_count,   cluster_max_count;
           pf_cluster_t * clusters;
          
           // Filter statistics
           pf_vector_t mean;
           pf_matrix_t cov;
           int converged;
          } pf_sample_set_t;
          
          
          // Information for an entire filter
          typedef struct _pf_t
          {
           // This min and max number of samples
           int min_samples,   max_samples;
          
           // Population size parameters
           double pop_err,   pop_z;
          
           // The sample sets. We keep two sets and use [current_set]
           // to identify the active set.
           int current_set;
           pf_sample_set_t sets[2];
          
           // Running averages,   slow and fast,   of likelihood
           double w_slow,   w_fast;
          
           // Decay rates for running averages
           double alpha_slow,   alpha_fast;
          
           // Function used to draw random pose samples
           pf_init_model_fn_t random_pose_fn;
           void * random_pose_data;
          
           double dist_threshold; // distance threshold in each axis over which the pf is considered to not
           // be converged
           int converged;
          } pf_t;
          
          
          // Create a new filter
     141  pf_t * pf_alloc(  
           int min_samples,   int max_samples,  
           double alpha_slow,   double alpha_fast,  
           pf_init_model_fn_t random_pose_fn,   void * random_pose_data );
          
          // Free an existing filter
     147  void pf_free(  pf_t * pf );
          
          // Initialize the filter using a guassian
     150  void pf_init(  pf_t * pf,   pf_vector_t mean,   pf_matrix_t cov );
          
          // Initialize the filter using some model
     153  void pf_init_model(  pf_t * pf,   pf_init_model_fn_t init_fn,   void * init_data );
          
          // Update the filter with some new action
          // void pf_update_action(  pf_t * pf,   pf_action_model_fn_t action_fn,   void * action_data );
          
          // Update the filter with some new sensor observation
     159  void pf_update_sensor(  pf_t * pf,   pf_sensor_model_fn_t sensor_fn,   void * sensor_data );
          
          // Resample the distribution
     162  void pf_update_resample(  pf_t * pf );
          
          // Compute the CEP statistics (  mean and variance ).
          // void pf_get_cep_stats(  pf_t * pf,   pf_vector_t * mean,   double * var );
          
          // Compute the statistics for a particular cluster. Returns 0 if
          // there is no such cluster.
     169  int pf_get_cluster_stats(  
           pf_t * pf,   int cluster,   double * weight,  
           pf_vector_t * mean,   pf_matrix_t * cov );
          
          // Re-compute the cluster statistics for a sample set
     174  void pf_cluster_stats(  pf_t * pf,   pf_sample_set_t * set );
          
          
          // Display the sample set
     178  void pf_draw_samples(  pf_t * pf,   struct _rtk_fig_t * fig,   int max_samples );
          
          // Draw the histogram (  kdtree )
     181  void pf_draw_hist(  pf_t * pf,   struct _rtk_fig_t * fig );
          
          // Draw the CEP statistics
          // void pf_draw_cep_stats(  pf_t * pf,   struct _rtk_fig_t * fig );
          
          // Draw the cluster statistics
     187  void pf_draw_cluster_stats(  pf_t * pf,   struct _rtk_fig_t * fig );
          
          // calculate if the particle filter has converged -
          // and sets the converged flag in the current set and the pf
     191  int pf_update_converged(  pf_t * pf );
          
          // sets the current set and pf converged values to zero
     194  void pf_init_converged(  pf_t * pf );
          
          #ifdef __cplusplus
          }
          #endif
          
          
          #endif // NAV2_AMCL__PF__PF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: KD tree functions
           * Author: Andrew Howard
           * Date: 18 Dec 2002
           * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
           *************************************************************************/
          
          #ifndef NAV2_AMCL__PF__PF_KDTREE_HPP_
          #define NAV2_AMCL__PF__PF_KDTREE_HPP_
          
          #ifdef INCLUDE_RTKGUI
          #include <rtk.h>
          #endif
          
          
          // Info for a node in the tree
          typedef struct pf_kdtree_node
          {
           // Depth in the tree
           int leaf,   depth;
          
           // Pivot dimension and value
           int pivot_dim;
           double pivot_value;
          
           // The key for this node
           int key[3];
          
           // The value for this node
           double value;
          
           // The cluster label (  leaf nodes )
           int cluster;
          
           // Child nodes
           struct pf_kdtree_node * children[2];
          } pf_kdtree_node_t;
          
          
          // A kd tree
          typedef struct
          {
           // Cell size
           double size[3];
          
           // The root node of the tree
           pf_kdtree_node_t * root;
          
           // The number of nodes in the tree
           int node_count,   node_max_count;
           pf_kdtree_node_t * nodes;
          
           // The number of leaf nodes in the tree
           int leaf_count;
          } pf_kdtree_t;
          
          
          // Create a tree
      79  extern pf_kdtree_t * pf_kdtree_alloc(  int max_size );
          
          // Destroy a tree
      82  extern void pf_kdtree_free(  pf_kdtree_t * self );
          
          // Clear all entries from the tree
      85  extern void pf_kdtree_clear(  pf_kdtree_t * self );
          
          // Insert a pose into the tree
      88  extern void pf_kdtree_insert(  pf_kdtree_t * self,   pf_vector_t pose,   double value );
          
          // Cluster the leaves in the tree
      91  extern void pf_kdtree_cluster(  pf_kdtree_t * self );
          
          // Determine the probability estimate for the given pose
          // extern double pf_kdtree_get_prob(  pf_kdtree_t * self,   pf_vector_t pose );
          
          // Determine the cluster label for the given pose
      97  extern int pf_kdtree_get_cluster(  pf_kdtree_t * self,   pf_vector_t pose );
          
          
          #ifdef INCLUDE_RTKGUI
          
          // Draw the tree
     103  extern void pf_kdtree_draw(  pf_kdtree_t * self,   rtk_fig_t * fig );
          
          #endif
          
          #endif // NAV2_AMCL__PF__PF_KDTREE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Useful pdf functions
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
           *************************************************************************/
          
          #ifndef NAV2_AMCL__PF__PF_PDF_HPP_
          #define NAV2_AMCL__PF__PF_PDF_HPP_
          
          #include "nav2_amcl/pf/pf_vector.hpp"
          
          // #include <gsl/gsl_rng.h>
          // #include <gsl/gsl_randist.h>
          
          #ifdef __cplusplus
          extern "C" {
          #endif
          
          /**************************************************************************
           * Gaussian
           *************************************************************************/
          
          // Gaussian PDF info
          typedef struct
          {
           // Mean,   covariance and inverse covariance
           pf_vector_t x;
           pf_matrix_t cx;
           // pf_matrix_t cxi;
           double cxdet;
          
           // Decomposed covariance matrix (  rotation * diagonal )
           pf_matrix_t cr;
           pf_vector_t cd;
          
           // A random number generator
           // gsl_rng *rng;
          } pf_pdf_gaussian_t;
          
          
          // Create a gaussian pdf
      63  pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(  pf_vector_t x,   pf_matrix_t cx );
          
          // Destroy the pdf
      66  void pf_pdf_gaussian_free(  pf_pdf_gaussian_t * pdf );
          
          // Compute the value of the pdf at some point [z].
          // double pf_pdf_gaussian_value(  pf_pdf_gaussian_t *pdf,   pf_vector_t z );
          
          // Draw randomly from a zero-mean Gaussian distribution,   with standard
          // deviation sigma.
          // We use the polar form of the Box-Muller transformation,   explained here:
          // http://www.taygeta.com/random/gaussian.html
      75  double pf_ran_gaussian(  double sigma );
          
          // Generate a sample from the pdf.
      78  pf_vector_t pf_pdf_gaussian_sample(  pf_pdf_gaussian_t * pdf );
          
          #ifdef __cplusplus
          }
          #endif
          
          #endif // NAV2_AMCL__PF__PF_PDF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Vector functions
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
           *************************************************************************/
          
          #ifndef NAV2_AMCL__PF__PF_VECTOR_HPP_
          #define NAV2_AMCL__PF__PF_VECTOR_HPP_
          
          #ifdef __cplusplus
          extern "C" {
          #endif
          
          #include <stdio.h>
          
          // The basic vector
          typedef struct
          {
           double v[3];
          } pf_vector_t;
          
          
          // The basic matrix
          typedef struct
          {
           double m[3][3];
          } pf_matrix_t;
          
          
          // Return a zero vector
      52  pf_vector_t pf_vector_zero(   );
          
          // Check for NAN or INF in any component
          // int pf_vector_finite(  pf_vector_t a );
          
          // Print a vector
          // void pf_vector_fprintf(  pf_vector_t s,   FILE * file,   const char * fmt );
          
          // Simple vector addition
          // pf_vector_t pf_vector_add(  pf_vector_t a,   pf_vector_t b );
          
          // Simple vector subtraction
      64  pf_vector_t pf_vector_sub(  pf_vector_t a,   pf_vector_t b );
          
          // Transform from local to global coords (  a + b )
      67  pf_vector_t pf_vector_coord_add(  pf_vector_t a,   pf_vector_t b );
          
          // Transform from global to local coords (  a - b )
          // pf_vector_t pf_vector_coord_sub(  pf_vector_t a,   pf_vector_t b );
          
          
          // Return a zero matrix
      74  pf_matrix_t pf_matrix_zero(   );
          
          // Check for NAN or INF in any component
          // int pf_matrix_finite(  pf_matrix_t a );
          
          // Print a matrix
          // void pf_matrix_fprintf(  pf_matrix_t s,   FILE * file,   const char * fmt );
          
          // Compute the matrix inverse. Will also return the determinant,  
          // which should be checked for underflow (  indicated singular matrix ).
          // pf_matrix_t pf_matrix_inverse(  pf_matrix_t a,   double *det );
          
          // Decompose a covariance matrix [a] into a rotation matrix [r] and a
          // diagonal matrix [d] such that a = r * d * r^T.
      88  void pf_matrix_unitary(  pf_matrix_t * r,   pf_matrix_t * d,   pf_matrix_t a );
          
          #ifdef __cplusplus
          }
          #endif
          
          #endif // NAV2_AMCL__PF__PF_VECTOR_HPP_

navigation2/nav2_amcl/include/nav2_amcl/portable_utils.hpp

       1  // Copyright (  c ) Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          #ifndef NAV2_AMCL__PORTABLE_UTILS_HPP_
          #define NAV2_AMCL__PORTABLE_UTILS_HPP_
          
          #include <stdlib.h>
          
          #ifdef __cplusplus
          extern "C" {
          #endif
          
          #ifndef HAVE_DRAND48
          // Some system (  e.g.,   Windows ) doesn't come with drand48(   ),   srand48(   ).
          // Use rand,   and srand for such system.
      26  static double drand48(  void )
          {
           return (  (  double )rand(   ) ) / RAND_MAX;// NOLINT
          }
          
      31  static void srand48(  long int seedval )// NOLINT
          {
           srand(  seedval );
          }
          #endif
          
          #ifdef __cplusplus
          }
          #endif
          
          #endif // NAV2_AMCL__PORTABLE_UTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // This library is free software; you can redistribute it and/or
          // modify it under the terms of the GNU Lesser General Public
          // License as published by the Free Software Foundation; either
          // version 2.1 of the License,   or (  at your option ) any later version.
          //
          // This library is distributed in the hope that it will be useful,  
          // but WITHOUT ANY WARRANTY; without even the implied warranty of
          // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
          // Lesser General Public License for more details.
          //
          // You should have received a copy of the GNU Lesser General Public
          // License along with this library; if not,   write to the Free Software
          // Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
          
          
          #ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
          #define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
          
          #include <string>
          #include "nav2_amcl/pf/pf.hpp"
          #include "nav2_amcl/pf/pf_pdf.hpp"
          #include "nav2_amcl/map/map.hpp"
          
          namespace nav2_amcl
          {
          
          // Forward declarations
      30  class LaserData;
          
          /*
           * @class Laser
           * @brief Base class for laser sensor models
           */
      36  class Laser
          {
          public:
           /**
           * @brief A Laser constructor
           * @param max_beams number of beams to use
           * @param map Map pointer to use
           */
      44   Laser(  size_t max_beams,   map_t * map );
          
           /*
           * @brief Laser destructor
           */
      49   virtual ~Laser(   );
          
           /*
           * @brief Run a sensor update on laser
           * @param pf Particle filter to use
           * @param data Laser data to use
           * @return if it was succesful
           */
      57   virtual bool sensorUpdate(  pf_t * pf,   LaserData * data ) = 0;
          
           /*
           * @brief Set the laser pose from an update
           * @param laser_pose Pose of the laser
           */
      63   void SetLaserPose(  pf_vector_t & laser_pose );
          
          protected:
           double z_hit_;
           double z_rand_;
           double sigma_hit_;
          
           /*
           * @brief Reallocate weights
           * @param max_samples Max number of samples
           * @param max_obs number of observations
           */
      75   void reallocTempData(  int max_samples,   int max_obs );
      76   map_t * map_;
      77   pf_vector_t laser_pose_;
           int max_beams_;
           int max_samples_;
           int max_obs_;
           double ** temp_obs_;
          };
          
          /*
           * @class LaserData
           * @brief Class of laser data to process
           */
      88  class LaserData
          {
          public:
      91   Laser * laser;
          
           /*
           * @brief LaserData constructor
           */
      96   LaserData(   ) {ranges = NULL;}
           /*
           * @brief LaserData destructor
           */
     100   virtual ~LaserData(   ) {delete[] ranges;}
          
          public:
           int range_count;
           double range_max;
           double(  *ranges )[2];
          };
          
          /*
           * @class BeamModel
           * @brief Beam model laser sensor
           */
     112  class BeamModel : public Laser
          {
          public:
           /*
           * @brief BeamModel constructor
           */
     118   BeamModel(  
           double z_hit,   double z_short,   double z_max,   double z_rand,   double sigma_hit,  
     120   double lambda_short,   double chi_outlier,   size_t max_beams,   map_t * map );
          
           /*
           * @brief Run a sensor update on laser
           * @param pf Particle filter to use
           * @param data Laser data to use
           * @return if it was succesful
           */
     128   bool sensorUpdate(  pf_t * pf,   LaserData * data );
          
          private:
     131   static double sensorFunction(  LaserData * data,   pf_sample_set_t * set );
           double z_short_;
           double z_max_;
           double lambda_short_;
           double chi_outlier_;
          };
          
          /*
           * @class LikelihoodFieldModel
           * @brief likelihood field model laser sensor
           */
     142  class LikelihoodFieldModel : public Laser
          {
          public:
           /*
           * @brief BeamModel constructor
           */
     148   LikelihoodFieldModel(  
           double z_hit,   double z_rand,   double sigma_hit,   double max_occ_dist,  
     150   size_t max_beams,   map_t * map );
          
           /*
           * @brief Run a sensor update on laser
           * @param pf Particle filter to use
           * @param data Laser data to use
           * @return if it was succesful
           */
     158   bool sensorUpdate(  pf_t * pf,   LaserData * data );
          
          private:
           /*
           * @brief Perform the update function
           * @param data Laser data to use
           * @param pf Particle filter to use
           * @return if it was succesful
           */
     167   static double sensorFunction(  LaserData * data,   pf_sample_set_t * set );
          };
          
          /*
           * @class LikelihoodFieldModelProb
           * @brief likelihood prob model laser sensor
           */
     174  class LikelihoodFieldModelProb : public Laser
          {
          public:
           /*
           * @brief BeamModel constructor
           */
     180   LikelihoodFieldModelProb(  
           double z_hit,   double z_rand,   double sigma_hit,   double max_occ_dist,  
     182   bool do_beamskip,   double beam_skip_distance,  
           double beam_skip_threshold,   double beam_skip_error_threshold,  
     184   size_t max_beams,   map_t * map );
          
           /*
           * @brief Run a sensor update on laser
           * @param pf Particle filter to use
           * @param data Laser data to use
           * @return if it was succesful
           */
     192   bool sensorUpdate(  pf_t * pf,   LaserData * data );
          
          private:
           /*
           * @brief Perform the update function
           * @param data Laser data to use
           * @param pf Particle filter to use
           * @return if it was succesful
           */
     201   static double sensorFunction(  LaserData * data,   pf_sample_set_t * set );
     202   bool do_beamskip_;
           double beam_skip_distance_;
           double beam_skip_threshold_;
           double beam_skip_error_threshold_;
          };
          
          } // namespace nav2_amcl
          
          #endif // NAV2_AMCL__SENSORS__LASER__LASER_HPP_

navigation2/nav2_amcl/src/amcl_node.cpp

          /*
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          /* Author: Brian Gerkey */
          
          #include "nav2_amcl/amcl_node.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "message_filters/subscriber.h"
          #include "nav2_amcl/angleutils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_amcl/pf/pf.hpp"
          #include "nav2_util/string_utils.hpp"
          #include "nav2_amcl/sensors/laser/laser.hpp"
          #include "tf2/convert.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "tf2/LinearMath/Transform.h"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/message_filter.h"
          #include "tf2_ros/transform_broadcaster.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          #include "nav2_amcl/portable_utils.hpp"
          
          using namespace std::placeholders;
          using rcl_interfaces::msg::ParameterType;
          using namespace std::chrono_literals;
          
          namespace nav2_amcl
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
      61  AmclNode::AmclNode(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "amcl",   "",   options )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           add_parameter(  
           "alpha1",   rclcpp::ParameterValue(  0.2 ),  
           "This is the alpha1 parameter",   "These are additional constraints for alpha1" );
          
           add_parameter(  
           "alpha2",   rclcpp::ParameterValue(  0.2 ),  
           "This is the alpha2 parameter",   "These are additional constraints for alpha2" );
          
           add_parameter(  
           "alpha3",   rclcpp::ParameterValue(  0.2 ),  
           "This is the alpha3 parameter",   "These are additional constraints for alpha3" );
          
           add_parameter(  
           "alpha4",   rclcpp::ParameterValue(  0.2 ),  
           "This is the alpha4 parameter",   "These are additional constraints for alpha4" );
          
           add_parameter(  
           "alpha5",   rclcpp::ParameterValue(  0.2 ),  
           "This is the alpha5 parameter",   "These are additional constraints for alpha5" );
          
           add_parameter(  
           "base_frame_id",   rclcpp::ParameterValue(  std::string(  "base_footprint" ) ),  
           "Which frame to use for the robot base" );
          
           add_parameter(  "beam_skip_distance",   rclcpp::ParameterValue(  0.5 ) );
           add_parameter(  "beam_skip_error_threshold",   rclcpp::ParameterValue(  0.9 ) );
           add_parameter(  "beam_skip_threshold",   rclcpp::ParameterValue(  0.3 ) );
           add_parameter(  "do_beamskip",   rclcpp::ParameterValue(  false ) );
          
           add_parameter(  
           "global_frame_id",   rclcpp::ParameterValue(  std::string(  "map" ) ),  
           "The name of the coordinate frame published by the localization system" );
          
           add_parameter(  
           "lambda_short",   rclcpp::ParameterValue(  0.1 ),  
           "Exponential decay parameter for z_short part of model" );
          
           add_parameter(  
           "laser_likelihood_max_dist",   rclcpp::ParameterValue(  2.0 ),  
           "Maximum distance to do obstacle inflation on map,   for use in likelihood_field model" );
          
           add_parameter(  
           "laser_max_range",   rclcpp::ParameterValue(  100.0 ),  
           "Maximum scan range to be considered",  
           "-1.0 will cause the laser's reported maximum range to be used" );
          
           add_parameter(  
           "laser_min_range",   rclcpp::ParameterValue(  -1.0 ),  
           "Minimum scan range to be considered",  
           "-1.0 will cause the laser's reported minimum range to be used" );
          
           add_parameter(  
           "laser_model_type",   rclcpp::ParameterValue(  std::string(  "likelihood_field" ) ),  
           "Which model to use,   either beam,   likelihood_field,   or likelihood_field_prob",  
           "Same as likelihood_field but incorporates the beamskip feature,   if enabled" );
          
           add_parameter(  
           "set_initial_pose",   rclcpp::ParameterValue(  false ),  
           "Causes AMCL to set initial pose from the initial_pose* parameters instead of "
           "waiting for the initial_pose message" );
          
           add_parameter(  
           "initial_pose.x",   rclcpp::ParameterValue(  0.0 ),  
           "X coordinate of the initial robot pose in the map frame" );
          
           add_parameter(  
           "initial_pose.y",   rclcpp::ParameterValue(  0.0 ),  
           "Y coordinate of the initial robot pose in the map frame" );
          
           add_parameter(  
           "initial_pose.z",   rclcpp::ParameterValue(  0.0 ),  
           "Z coordinate of the initial robot pose in the map frame" );
          
           add_parameter(  
           "initial_pose.yaw",   rclcpp::ParameterValue(  0.0 ),  
           "Yaw of the initial robot pose in the map frame" );
          
           add_parameter(  
           "max_beams",   rclcpp::ParameterValue(  60 ),  
           "How many evenly-spaced beams in each scan to be used when updating the filter" );
          
           add_parameter(  
           "max_particles",   rclcpp::ParameterValue(  2000 ),  
           "Minimum allowed number of particles" );
          
           add_parameter(  
           "min_particles",   rclcpp::ParameterValue(  500 ),  
           "Maximum allowed number of particles" );
          
           add_parameter(  
           "odom_frame_id",   rclcpp::ParameterValue(  std::string(  "odom" ) ),  
           "Which frame to use for odometry" );
          
           add_parameter(  "pf_err",   rclcpp::ParameterValue(  0.05 ) );
           add_parameter(  "pf_z",   rclcpp::ParameterValue(  0.99 ) );
          
           add_parameter(  
           "recovery_alpha_fast",   rclcpp::ParameterValue(  0.0 ),  
           "Exponential decay rate for the fast average weight filter,   used in deciding when to recover "
           "by adding random poses",  
           "A good value might be 0.1" );
          
           add_parameter(  
           "recovery_alpha_slow",   rclcpp::ParameterValue(  0.0 ),  
           "Exponential decay rate for the slow average weight filter,   used in deciding when to recover "
           "by adding random poses",  
           "A good value might be 0.001" );
          
           add_parameter(  
           "resample_interval",   rclcpp::ParameterValue(  1 ),  
           "Number of filter updates required before resampling" );
          
           add_parameter(  "robot_model_type",   rclcpp::ParameterValue(  "nav2_amcl::DifferentialMotionModel" ) );
          
           add_parameter(  
           "save_pose_rate",   rclcpp::ParameterValue(  0.5 ),  
           "Maximum rate (  Hz ) at which to store the last estimated pose and covariance to the parameter "
           "server,   in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used "
           "on subsequent runs to initialize the filter",  
           "-1.0 to disable" );
          
           add_parameter(  "sigma_hit",   rclcpp::ParameterValue(  0.2 ) );
          
           add_parameter(  
           "tf_broadcast",   rclcpp::ParameterValue(  true ),  
           "Set this to false to prevent amcl from publishing the transform between the global frame and "
           "the odometry frame" );
          
           add_parameter(  
           "transform_tolerance",   rclcpp::ParameterValue(  1.0 ),  
           "Time with which to post-date the transform that is published,   to indicate that this transform "
           "is valid into the future" );
          
           add_parameter(  
           "update_min_a",   rclcpp::ParameterValue(  0.2 ),  
           "Rotational movement required before performing a filter update" );
          
           add_parameter(  
           "update_min_d",   rclcpp::ParameterValue(  0.25 ),  
           "Translational movement required before performing a filter update" );
          
           add_parameter(  "z_hit",   rclcpp::ParameterValue(  0.5 ) );
           add_parameter(  "z_max",   rclcpp::ParameterValue(  0.05 ) );
           add_parameter(  "z_rand",   rclcpp::ParameterValue(  0.5 ) );
           add_parameter(  "z_short",   rclcpp::ParameterValue(  0.05 ) );
          
           add_parameter(  
           "always_reset_initial_pose",   rclcpp::ParameterValue(  false ),  
           "Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter "
           "(  with parameter set_initial_pose: true ) when reset. Otherwise,   by default AMCL will use the"
           "last known pose to initialize" );
          
           add_parameter(  
           "scan_topic",   rclcpp::ParameterValue(  "scan" ),  
           "Topic to subscribe to in order to receive the laser scan for localization" );
          
           add_parameter(  
           "map_topic",   rclcpp::ParameterValue(  "map" ),  
           "Topic to subscribe to in order to receive the map to localize on" );
          
           add_parameter(  
           "first_map_only",   rclcpp::ParameterValue(  false ),  
           "Set this to true,   when you want to load a new map published from the map_server" );
          }
          
     231  AmclNode::~AmclNode(   )
          {
          }
          
          nav2_util::CallbackReturn
     236  AmclNode::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
           callback_group_ = create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,   false );
           initParameters(   );
           initTransforms(   );
           initParticleFilter(   );
           initLaserScan(   );
           initMessageFilters(   );
           initPubSub(   );
           initServices(   );
           initOdometry(   );
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           executor_->add_callback_group(  callback_group_,   get_node_base_interface(   ) );
           executor_thread_ = std::make_unique<nav2_util::NodeThread>(  executor_ );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     256  AmclNode::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           // Lifecycle publishers must be explicitly activated
           pose_pub_->on_activate(   );
           particle_cloud_pub_->on_activate(   );
          
           first_pose_sent_ = false;
          
           // Keep track of whether we're in the active state. We won't
           // process incoming callbacks until we are
           active_ = true;
          
           if (  set_initial_pose_ ) {
           auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(   );
          
           msg->header.stamp = now(   );
           msg->header.frame_id = global_frame_id_;
           msg->pose.pose.position.x = initial_pose_x_;
           msg->pose.pose.position.y = initial_pose_y_;
           msg->pose.pose.position.z = initial_pose_z_;
           msg->pose.pose.orientation = orientationAroundZAxis(  initial_pose_yaw_ );
          
           initialPoseReceived(  msg );
           } else if (  init_pose_received_on_inactive ) {
           handleInitialPose(  last_published_pose_ );
           }
          
           auto node = shared_from_this(   );
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &AmclNode::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     299  AmclNode::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           active_ = false;
          
           // Lifecycle publishers must be explicitly deactivated
           pose_pub_->on_deactivate(   );
           particle_cloud_pub_->on_deactivate(   );
          
           // reset dynamic parameter handler
           dyn_params_handler_.reset(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     319  AmclNode::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           executor_thread_.reset(   );
          
           // Get rid of the inputs first (  services and message filter input ),   so we
           // don't continue to process incoming messages
           global_loc_srv_.reset(   );
           nomotion_update_srv_.reset(   );
           initial_pose_sub_.reset(   );
           laser_scan_connection_.disconnect(   );
           laser_scan_filter_.reset(   );
           laser_scan_sub_.reset(   );
          
           // Map
           if (  map_ != NULL ) {
           map_free(  map_ );
           map_ = nullptr;
           }
           first_map_received_ = false;
           free_space_indices.resize(  0 );
          
           // Transforms
           tf_broadcaster_.reset(   );
           tf_listener_.reset(   );
           tf_buffer_.reset(   );
          
           // PubSub
           pose_pub_.reset(   );
           particle_cloud_pub_.reset(   );
          
           // Odometry
           motion_model_.reset(   );
          
           // Particle Filter
           pf_free(  pf_ );
           pf_ = nullptr;
          
           // Laser Scan
           lasers_.clear(   );
           lasers_update_.clear(   );
           frame_to_laser_.clear(   );
           force_update_ = true;
          
           if (  set_initial_pose_ ) {
           set_parameter(  
           rclcpp::Parameter(  
           "initial_pose.x",  
           rclcpp::ParameterValue(  last_published_pose_.pose.pose.position.x ) ) );
           set_parameter(  
           rclcpp::Parameter(  
           "initial_pose.y",  
           rclcpp::ParameterValue(  last_published_pose_.pose.pose.position.y ) ) );
           set_parameter(  
           rclcpp::Parameter(  
           "initial_pose.z",  
           rclcpp::ParameterValue(  last_published_pose_.pose.pose.position.z ) ) );
           set_parameter(  
           rclcpp::Parameter(  
           "initial_pose.yaw",  
           rclcpp::ParameterValue(  tf2::getYaw(  last_published_pose_.pose.pose.orientation ) ) ) );
           }
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     387  AmclNode::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          bool
     394  AmclNode::checkElapsedTime(  std::chrono::seconds check_interval,   rclcpp::Time last_time )
          {
           rclcpp::Duration elapsed_time = now(   ) - last_time;
           if (  elapsed_time.nanoseconds(   ) * 1e-9 > check_interval.count(   ) ) {
           return true;
           }
           return false;
          }
          
          #if NEW_UNIFORM_SAMPLING
     404  std::vector<std::pair<int,   int>> AmclNode::free_space_indices;
          #endif
          
          bool
     408  AmclNode::getOdomPose(  
     409   geometry_msgs::msg::PoseStamped & odom_pose,  
           double & x,   double & y,   double & yaw,  
     411   const rclcpp::Time & sensor_timestamp,   const std::string & frame_id )
          {
           // Get the robot's pose
           geometry_msgs::msg::PoseStamped ident;
           ident.header.frame_id = nav2_util::strip_leading_slash(  frame_id );
           ident.header.stamp = sensor_timestamp;
           tf2::toMsg(  tf2::Transform::getIdentity(   ),   ident.pose );
          
           try {
           tf_buffer_->transform(  ident,   odom_pose,   odom_frame_id_ );
           } catch (  tf2::TransformException & e ) {
           ++scan_error_count_;
           if (  scan_error_count_ % 20 == 0 ) {
           RCLCPP_ERROR(  
           get_logger(   ),   "(  %d ) consecutive laser scan transforms failed: (  %s )",   scan_error_count_,  
           e.what(   ) );
           }
           return false;
           }
          
           scan_error_count_ = 0; // reset since we got a good transform
           x = odom_pose.pose.position.x;
           y = odom_pose.pose.position.y;
           yaw = tf2::getYaw(  odom_pose.pose.orientation );
          
           return true;
          }
          
          pf_vector_t
     440  AmclNode::uniformPoseGenerator(  void * arg )
          {
           map_t * map = reinterpret_cast<map_t *>(  arg );
          
          #if NEW_UNIFORM_SAMPLING
           unsigned int rand_index = drand48(   ) * free_space_indices.size(   );
           std::pair<int,   int> free_point = free_space_indices[rand_index];
           pf_vector_t p;
           p.v[0] = MAP_WXGX(  map,   free_point.first );
           p.v[1] = MAP_WYGY(  map,   free_point.second );
           p.v[2] = drand48(   ) * 2 * M_PI - M_PI;
          #else
           double min_x,   max_x,   min_y,   max_y;
          
           min_x = (  map->size_x * map->scale ) / 2.0 - map->origin_x;
           max_x = (  map->size_x * map->scale ) / 2.0 + map->origin_x;
           min_y = (  map->size_y * map->scale ) / 2.0 - map->origin_y;
           max_y = (  map->size_y * map->scale ) / 2.0 + map->origin_y;
          
           pf_vector_t p;
          
           RCLCPP_DEBUG(  get_logger(   ),   "Generating new uniform sample" );
           for (  ;;  ) {
           p.v[0] = min_x + drand48(   ) * (  max_x - min_x );
           p.v[1] = min_y + drand48(   ) * (  max_y - min_y );
           p.v[2] = drand48(   ) * 2 * M_PI - M_PI;
           // Check that it's a free cell
           int i,   j;
           i = MAP_GXWX(  map,   p.v[0] );
           j = MAP_GYWY(  map,   p.v[1] );
           if (  MAP_VALID(  map,   i,   j ) && (  map->cells[MAP_INDEX(  map,   i,   j )].occ_state == -1 ) ) {
           break;
           }
           }
          #endif
           return p;
          }
          
          void
     479  AmclNode::globalLocalizationCallback(  
     480   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     481   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,  
     482   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/ )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
          
           RCLCPP_INFO(  get_logger(   ),   "Initializing with uniform distribution" );
          
           pf_init_model(  
           pf_,   (  pf_init_model_fn_t )AmclNode::uniformPoseGenerator,  
           reinterpret_cast<void *>(  map_ ) );
           RCLCPP_INFO(  get_logger(   ),   "Global initialisation done!" );
           initial_pose_is_known_ = true;
           pf_init_ = false;
          }
          
          // force nomotion updates (  amcl updating without requiring motion )
          void
     498  AmclNode::nomotionUpdateCallback(  
     499   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     500   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,  
     501   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Requesting no-motion update" );
           force_update_ = true;
          }
          
          void
     508  AmclNode::initialPoseReceived(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
          
           RCLCPP_INFO(  get_logger(   ),   "initialPoseReceived" );
          
           if (  msg->header.frame_id == "" ) {
           // This should be removed at some point
           RCLCPP_WARN(  
           get_logger(   ),  
           "Received initial pose with empty frame_id. You should always supply a frame_id." );
           return;
           }
           if (  nav2_util::strip_leading_slash(  msg->header.frame_id ) != global_frame_id_ ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame,   \"%s\"",  
           nav2_util::strip_leading_slash(  msg->header.frame_id ).c_str(   ),  
           global_frame_id_.c_str(   ) );
           return;
           }
           // Overriding last published pose to initial pose
           last_published_pose_ = *msg;
          
           if (  !active_ ) {
           init_pose_received_on_inactive = true;
           RCLCPP_WARN(  
           get_logger(   ),   "Received initial pose request,   "
           "but AMCL is not yet in the active state" );
           return;
           }
           handleInitialPose(  *msg );
          }
          
          void
     543  AmclNode::handleInitialPose(  geometry_msgs::msg::PoseWithCovarianceStamped & msg )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
           // In case the client sent us a pose estimate in the past,   integrate the
           // intervening odometric change.
           geometry_msgs::msg::TransformStamped tx_odom;
           try {
           rclcpp::Time rclcpp_time = now(   );
           tf2::TimePoint tf2_time(  std::chrono::nanoseconds(  rclcpp_time.nanoseconds(   ) ) );
          
           // Check if the transform is available
           tx_odom = tf_buffer_->lookupTransform(  
           base_frame_id_,   tf2_ros::fromMsg(  msg.header.stamp ),  
           base_frame_id_,   tf2_time,   odom_frame_id_ );
           } catch (  tf2::TransformException & e ) {
           // If we've never sent a transform,   then this is normal,   because the
           // global_frame_id_ frame doesn't exist. We only care about in-time
           // transformation for on-the-move pose-setting,   so ignoring this
           // startup condition doesn't really cost us anything.
           if (  sent_first_transform_ ) {
           RCLCPP_WARN(  get_logger(   ),   "Failed to transform initial pose in time (  %s )",   e.what(   ) );
           }
           tf2::impl::Converter<false,   true>::convert(  tf2::Transform::getIdentity(   ),   tx_odom.transform );
           }
          
           tf2::Transform tx_odom_tf2;
           tf2::impl::Converter<true,   false>::convert(  tx_odom.transform,   tx_odom_tf2 );
          
           tf2::Transform pose_old;
           tf2::impl::Converter<true,   false>::convert(  msg.pose.pose,   pose_old );
          
           tf2::Transform pose_new = pose_old * tx_odom_tf2;
          
           // Transform into the global frame
          
           RCLCPP_INFO(  
           get_logger(   ),   "Setting pose (  %.6f ): %.3f %.3f %.3f",  
           now(   ).nanoseconds(   ) * 1e-9,  
           pose_new.getOrigin(   ).x(   ),  
           pose_new.getOrigin(   ).y(   ),  
           tf2::getYaw(  pose_new.getRotation(   ) ) );
          
           // Re-initialize the filter
           pf_vector_t pf_init_pose_mean = pf_vector_zero(   );
           pf_init_pose_mean.v[0] = pose_new.getOrigin(   ).x(   );
           pf_init_pose_mean.v[1] = pose_new.getOrigin(   ).y(   );
           pf_init_pose_mean.v[2] = tf2::getYaw(  pose_new.getRotation(   ) );
          
           pf_matrix_t pf_init_pose_cov = pf_matrix_zero(   );
           // Copy in the covariance,   converting from 6-D to 3-D
           for (  int i = 0; i < 2; i++ ) {
           for (  int j = 0; j < 2; j++ ) {
           pf_init_pose_cov.m[i][j] = msg.pose.covariance[6 * i + j];
           }
           }
          
           pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5];
          
           pf_init(  pf_,   pf_init_pose_mean,   pf_init_pose_cov );
           pf_init_ = false;
           init_pose_received_on_inactive = false;
           initial_pose_is_known_ = true;
          }
          
          void
     608  AmclNode::laserReceived(  sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
          
           // Since the sensor data is continually being published by the simulator or robot,  
           // we don't want our callbacks to fire until we're in the active state
           if (  !active_ ) {return;}
           if (  !first_map_received_ ) {
           if (  checkElapsedTime(  2s,   last_time_printed_msg_ ) ) {
           RCLCPP_WARN(  get_logger(   ),   "Waiting for map...." );
           last_time_printed_msg_ = now(   );
           }
           return;
           }
          
           std::string laser_scan_frame_id = nav2_util::strip_leading_slash(  laser_scan->header.frame_id );
           last_laser_received_ts_ = now(   );
           int laser_index = -1;
           geometry_msgs::msg::PoseStamped laser_pose;
          
           // Do we have the base->base_laser Tx yet?
           if (  frame_to_laser_.find(  laser_scan_frame_id ) == frame_to_laser_.end(   ) ) {
           if (  !addNewScanner(  laser_index,   laser_scan,   laser_scan_frame_id,   laser_pose ) ) {
           return; // could not find transform
           }
           } else {
           // we have the laser pose,   retrieve laser index
           laser_index = frame_to_laser_[laser_scan->header.frame_id];
           }
          
           // Where was the robot when this scan was taken?
           pf_vector_t pose;
           if (  !getOdomPose(  
           latest_odom_pose_,   pose.v[0],   pose.v[1],   pose.v[2],  
           laser_scan->header.stamp,   base_frame_id_ ) )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Couldn't determine robot's pose associated with laser scan" );
           return;
           }
          
           pf_vector_t delta = pf_vector_zero(   );
           bool force_publication = false;
           if (  !pf_init_ ) {
           // Pose at last filter update
           pf_odom_pose_ = pose;
           pf_init_ = true;
          
           for (  unsigned int i = 0; i < lasers_update_.size(   ); i++ ) {
           lasers_update_[i] = true;
           }
          
           force_publication = true;
           resample_count_ = 0;
           } else {
           // Set the laser update flags
           if (  shouldUpdateFilter(  pose,   delta ) ) {
           for (  unsigned int i = 0; i < lasers_update_.size(   ); i++ ) {
           lasers_update_[i] = true;
           }
           }
           if (  lasers_update_[laser_index] ) {
           motion_model_->odometryUpdate(  pf_,   pose,   delta );
           }
           force_update_ = false;
           }
          
           bool resampled = false;
          
           // If the robot has moved,   update the filter
           if (  lasers_update_[laser_index] ) {
           updateFilter(  laser_index,   laser_scan,   pose );
          
           // Resample the particles
           if (  !(  ++resample_count_ % resample_interval_ ) ) {
           pf_update_resample(  pf_ );
           resampled = true;
           }
          
           pf_sample_set_t * set = pf_->sets + pf_->current_set;
           RCLCPP_DEBUG(  get_logger(   ),   "Num samples: %d\n",   set->sample_count );
          
           if (  !force_update_ ) {
           publishParticleCloud(  set );
           }
           }
           if (  resampled || force_publication || !first_pose_sent_ ) {
           amcl_hyp_t max_weight_hyps;
           std::vector<amcl_hyp_t> hyps;
           int max_weight_hyp = -1;
           if (  getMaxWeightHyp(  hyps,   max_weight_hyps,   max_weight_hyp ) ) {
           publishAmclPose(  laser_scan,   hyps,   max_weight_hyp );
           calculateMaptoOdomTransform(  laser_scan,   hyps,   max_weight_hyp );
          
           if (  tf_broadcast_ == true ) {
           // We want to send a transform that is good up until a
           // tolerance time so that odom can be used
           auto stamp = tf2_ros::fromMsg(  laser_scan->header.stamp );
           tf2::TimePoint transform_expiration = stamp + transform_tolerance_;
           sendMapToOdomTransform(  transform_expiration );
           sent_first_transform_ = true;
           }
           } else {
           RCLCPP_ERROR(  get_logger(   ),   "No pose!" );
           }
           } else if (  latest_tf_valid_ ) {
           if (  tf_broadcast_ == true ) {
           // Nothing changed,   so we'll just republish the last transform,   to keep
           // everybody happy.
           tf2::TimePoint transform_expiration = tf2_ros::fromMsg(  laser_scan->header.stamp ) +
           transform_tolerance_;
           sendMapToOdomTransform(  transform_expiration );
           }
           }
          }
          
     723  bool AmclNode::addNewScanner(  
           int & laser_index,  
     725   const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
     726   const std::string & laser_scan_frame_id,  
     727   geometry_msgs::msg::PoseStamped & laser_pose )
          {
           lasers_.push_back(  createLaserObject(   ) );
           lasers_update_.push_back(  true );
           laser_index = frame_to_laser_.size(   );
          
           geometry_msgs::msg::PoseStamped ident;
           ident.header.frame_id = laser_scan_frame_id;
           ident.header.stamp = rclcpp::Time(   );
           tf2::toMsg(  tf2::Transform::getIdentity(   ),   ident.pose );
           try {
           tf_buffer_->transform(  ident,   laser_pose,   base_frame_id_,   transform_tolerance_ );
           } catch (  tf2::TransformException & e ) {
           RCLCPP_ERROR(  
           get_logger(   ),   "Couldn't transform from %s to %s,   "
           "even though the message notifier is in use: (  %s )",  
           laser_scan->header.frame_id.c_str(   ),  
           base_frame_id_.c_str(   ),   e.what(   ) );
           return false;
           }
          
           pf_vector_t laser_pose_v;
           laser_pose_v.v[0] = laser_pose.pose.position.x;
           laser_pose_v.v[1] = laser_pose.pose.position.y;
           // laser mounting angle gets computed later -> set to 0 here!
           laser_pose_v.v[2] = 0;
           lasers_[laser_index]->SetLaserPose(  laser_pose_v );
           frame_to_laser_[laser_scan->header.frame_id] = laser_index;
           return true;
          }
          
     758  bool AmclNode::shouldUpdateFilter(  const pf_vector_t pose,   pf_vector_t & delta )
          {
           delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
           delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
           delta.v[2] = angleutils::angle_diff(  pose.v[2],   pf_odom_pose_.v[2] );
          
           // See if we should update the filter
           bool update = fabs(  delta.v[0] ) > d_thresh_ ||
           fabs(  delta.v[1] ) > d_thresh_ ||
           fabs(  delta.v[2] ) > a_thresh_;
           update = update || force_update_;
           return update;
          }
          
     772  bool AmclNode::updateFilter(  
           const int & laser_index,  
     774   const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
     775   const pf_vector_t & pose )
          {
           nav2_amcl::LaserData ldata;
           ldata.laser = lasers_[laser_index];
           ldata.range_count = laser_scan->ranges.size(   );
           // To account for lasers that are mounted upside-down,   we determine the
           // min,   max,   and increment angles of the laser in the base frame.
           //
           // Construct min and max angles of laser,   in the base_link frame.
           // Here we set the roll pich yaw of the lasers. We assume roll and pich are zero.
           geometry_msgs::msg::QuaternionStamped min_q,   inc_q;
           min_q.header.stamp = laser_scan->header.stamp;
           min_q.header.frame_id = nav2_util::strip_leading_slash(  laser_scan->header.frame_id );
           min_q.quaternion = orientationAroundZAxis(  laser_scan->angle_min );
          
           inc_q.header = min_q.header;
           inc_q.quaternion = orientationAroundZAxis(  laser_scan->angle_min + laser_scan->angle_increment );
           try {
           tf_buffer_->transform(  min_q,   min_q,   base_frame_id_ );
           tf_buffer_->transform(  inc_q,   inc_q,   base_frame_id_ );
           } catch (  tf2::TransformException & e ) {
           RCLCPP_WARN(  
           get_logger(   ),   "Unable to transform min/max laser angles into base frame: %s",  
           e.what(   ) );
           return false;
           }
           double angle_min = tf2::getYaw(  min_q.quaternion );
           double angle_increment = tf2::getYaw(  inc_q.quaternion ) - angle_min;
          
           // wrapping angle to [-pi .. pi]
           angle_increment = fmod(  angle_increment + 5 * M_PI,   2 * M_PI ) - M_PI;
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "Laser %d angles in base frame: min: %.3f inc: %.3f",   laser_index,   angle_min,  
           angle_increment );
          
           // Apply range min/max thresholds,   if the user supplied them
           if (  laser_max_range_ > 0.0 ) {
           ldata.range_max = std::min(  laser_scan->range_max,   static_cast<float>(  laser_max_range_ ) );
           } else {
           ldata.range_max = laser_scan->range_max;
           }
           double range_min;
           if (  laser_min_range_ > 0.0 ) {
           range_min = std::max(  laser_scan->range_min,   static_cast<float>(  laser_min_range_ ) );
           } else {
           range_min = laser_scan->range_min;
           }
          
           // The LaserData destructor will free this memory
           ldata.ranges = new double[ldata.range_count][2];
           for (  int i = 0; i < ldata.range_count; i++ ) {
           // amcl doesn't (  yet ) have a concept of min range. So we'll map short
           // readings to max range.
           if (  laser_scan->ranges[i] <= range_min ) {
           ldata.ranges[i][0] = ldata.range_max;
           } else {
           ldata.ranges[i][0] = laser_scan->ranges[i];
           }
           // Compute bearing
           ldata.ranges[i][1] = angle_min +
           (  i * angle_increment );
           }
           lasers_[laser_index]->sensorUpdate(  pf_,   reinterpret_cast<nav2_amcl::LaserData *>(  &ldata ) );
           lasers_update_[laser_index] = false;
           pf_odom_pose_ = pose;
           return true;
          }
          
          void
     845  AmclNode::publishParticleCloud(  const pf_sample_set_t * set )
          {
           // If initial pose is not known,   AMCL does not know the current pose
           if (  !initial_pose_is_known_ ) {return;}
           auto cloud_with_weights_msg = std::make_unique<nav2_msgs::msg::ParticleCloud>(   );
           cloud_with_weights_msg->header.stamp = this->now(   );
           cloud_with_weights_msg->header.frame_id = global_frame_id_;
           cloud_with_weights_msg->particles.resize(  set->sample_count );
          
           for (  int i = 0; i < set->sample_count; i++ ) {
           cloud_with_weights_msg->particles[i].pose.position.x = set->samples[i].pose.v[0];
           cloud_with_weights_msg->particles[i].pose.position.y = set->samples[i].pose.v[1];
           cloud_with_weights_msg->particles[i].pose.position.z = 0;
           cloud_with_weights_msg->particles[i].pose.orientation = orientationAroundZAxis(  
           set->samples[i].pose.v[2] );
           cloud_with_weights_msg->particles[i].weight = set->samples[i].weight;
           }
          
           particle_cloud_pub_->publish(  std::move(  cloud_with_weights_msg ) );
          }
          
          bool
     867  AmclNode::getMaxWeightHyp(  
     868   std::vector<amcl_hyp_t> & hyps,   amcl_hyp_t & max_weight_hyps,  
           int & max_weight_hyp )
          {
           // Read out the current hypotheses
           double max_weight = 0.0;
           hyps.resize(  pf_->sets[pf_->current_set].cluster_count );
           for (  int hyp_count = 0;
           hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++ )
           {
           double weight;
           pf_vector_t pose_mean;
           pf_matrix_t pose_cov;
           if (  !pf_get_cluster_stats(  pf_,   hyp_count,   &weight,   &pose_mean,   &pose_cov ) ) {
           RCLCPP_ERROR(  get_logger(   ),   "Couldn't get stats on cluster %d",   hyp_count );
           return false;
           }
          
           hyps[hyp_count].weight = weight;
           hyps[hyp_count].pf_pose_mean = pose_mean;
           hyps[hyp_count].pf_pose_cov = pose_cov;
          
           if (  hyps[hyp_count].weight > max_weight ) {
           max_weight = hyps[hyp_count].weight;
           max_weight_hyp = hyp_count;
           }
           }
          
           if (  max_weight > 0.0 ) {
           RCLCPP_DEBUG(  
           get_logger(   ),   "Max weight pose: %.3f %.3f %.3f",  
           hyps[max_weight_hyp].pf_pose_mean.v[0],  
           hyps[max_weight_hyp].pf_pose_mean.v[1],  
           hyps[max_weight_hyp].pf_pose_mean.v[2] );
          
           max_weight_hyps = hyps[max_weight_hyp];
           return true;
           }
           return false;
          }
          
          void
     909  AmclNode::publishAmclPose(  
     910   const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
     911   const std::vector<amcl_hyp_t> & hyps,   const int & max_weight_hyp )
          {
           // If initial pose is not known,   AMCL does not know the current pose
           if (  !initial_pose_is_known_ ) {
           if (  checkElapsedTime(  2s,   last_time_printed_msg_ ) ) {
           RCLCPP_WARN(  
           get_logger(   ),   "AMCL cannot publish a pose or update the transform. "
           "Please set the initial pose..." );
           last_time_printed_msg_ = now(   );
           }
           return;
           }
          
           auto p = std::make_unique<geometry_msgs::msg::PoseWithCovarianceStamped>(   );
           // Fill in the header
           p->header.frame_id = global_frame_id_;
           p->header.stamp = laser_scan->header.stamp;
           // Copy in the pose
           p->pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
           p->pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
           p->pose.pose.orientation = orientationAroundZAxis(  hyps[max_weight_hyp].pf_pose_mean.v[2] );
           // Copy in the covariance,   converting from 3-D to 6-D
           pf_sample_set_t * set = pf_->sets + pf_->current_set;
           for (  int i = 0; i < 2; i++ ) {
           for (  int j = 0; j < 2; j++ ) {
           // Report the overall filter covariance,   rather than the
           // covariance for the highest-weight cluster
           // p->covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
           p->pose.covariance[6 * i + j] = set->cov.m[i][j];
           }
           }
           p->pose.covariance[6 * 5 + 5] = set->cov.m[2][2];
           float temp = 0.0;
           for (  auto covariance_value : p->pose.covariance ) {
           temp += covariance_value;
           }
           temp += p->pose.pose.position.x + p->pose.pose.position.y;
           if (  !std::isnan(  temp ) ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Publishing pose" );
           last_published_pose_ = *p;
           first_pose_sent_ = true;
           pose_pub_->publish(  std::move(  p ) );
           } else {
           RCLCPP_WARN(  
           get_logger(   ),   "AMCL covariance or pose is NaN,   likely due to an invalid "
           "configuration or faulty sensor measurements! Pose is not available!" );
           }
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "New pose: %6.3f %6.3f %6.3f",  
           hyps[max_weight_hyp].pf_pose_mean.v[0],  
           hyps[max_weight_hyp].pf_pose_mean.v[1],  
           hyps[max_weight_hyp].pf_pose_mean.v[2] );
          }
          
          void
     967  AmclNode::calculateMaptoOdomTransform(  
     968   const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,  
     969   const std::vector<amcl_hyp_t> & hyps,   const int & max_weight_hyp )
          {
           // subtracting base to odom from map to base and send map to odom instead
           geometry_msgs::msg::PoseStamped odom_to_map;
           try {
           tf2::Quaternion q;
           q.setRPY(  0,   0,   hyps[max_weight_hyp].pf_pose_mean.v[2] );
           tf2::Transform tmp_tf(  q,   tf2::Vector3(  
           hyps[max_weight_hyp].pf_pose_mean.v[0],  
           hyps[max_weight_hyp].pf_pose_mean.v[1],  
           0.0 ) );
          
           geometry_msgs::msg::PoseStamped tmp_tf_stamped;
           tmp_tf_stamped.header.frame_id = base_frame_id_;
           tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
           tf2::toMsg(  tmp_tf.inverse(   ),   tmp_tf_stamped.pose );
          
           tf_buffer_->transform(  tmp_tf_stamped,   odom_to_map,   odom_frame_id_ );
           } catch (  tf2::TransformException & e ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Failed to subtract base to odom transform: (  %s )",   e.what(   ) );
           return;
           }
          
           tf2::impl::Converter<true,   false>::convert(  odom_to_map.pose,   latest_tf_ );
           latest_tf_valid_ = true;
          }
          
          void
     997  AmclNode::sendMapToOdomTransform(  const tf2::TimePoint & transform_expiration )
          {
           // AMCL will update transform only when it has knowledge about robot's initial position
           if (  !initial_pose_is_known_ ) {return;}
           geometry_msgs::msg::TransformStamped tmp_tf_stamped;
           tmp_tf_stamped.header.frame_id = global_frame_id_;
           tmp_tf_stamped.header.stamp = tf2_ros::toMsg(  transform_expiration );
           tmp_tf_stamped.child_frame_id = odom_frame_id_;
           tf2::impl::Converter<false,   true>::convert(  latest_tf_.inverse(   ),   tmp_tf_stamped.transform );
           tf_broadcaster_->sendTransform(  tmp_tf_stamped );
          }
          
          nav2_amcl::Laser *
    1010  AmclNode::createLaserObject(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "createLaserObject" );
          
           if (  sensor_model_type_ == "beam" ) {
           return new nav2_amcl::BeamModel(  
           z_hit_,   z_short_,   z_max_,   z_rand_,   sigma_hit_,   lambda_short_,  
           0.0,   max_beams_,   map_ );
           }
          
           if (  sensor_model_type_ == "likelihood_field_prob" ) {
           return new nav2_amcl::LikelihoodFieldModelProb(  
           z_hit_,   z_rand_,   sigma_hit_,  
           laser_likelihood_max_dist_,   do_beamskip_,   beam_skip_distance_,   beam_skip_threshold_,  
           beam_skip_error_threshold_,   max_beams_,   map_ );
           }
          
           return new nav2_amcl::LikelihoodFieldModel(  
           z_hit_,   z_rand_,   sigma_hit_,  
           laser_likelihood_max_dist_,   max_beams_,   map_ );
          }
          
          void
    1033  AmclNode::initParameters(   )
          {
           double save_pose_rate;
           double tmp_tol;
          
           get_parameter(  "alpha1",   alpha1_ );
           get_parameter(  "alpha2",   alpha2_ );
           get_parameter(  "alpha3",   alpha3_ );
           get_parameter(  "alpha4",   alpha4_ );
           get_parameter(  "alpha5",   alpha5_ );
           get_parameter(  "base_frame_id",   base_frame_id_ );
           get_parameter(  "beam_skip_distance",   beam_skip_distance_ );
           get_parameter(  "beam_skip_error_threshold",   beam_skip_error_threshold_ );
           get_parameter(  "beam_skip_threshold",   beam_skip_threshold_ );
           get_parameter(  "do_beamskip",   do_beamskip_ );
           get_parameter(  "global_frame_id",   global_frame_id_ );
           get_parameter(  "lambda_short",   lambda_short_ );
           get_parameter(  "laser_likelihood_max_dist",   laser_likelihood_max_dist_ );
           get_parameter(  "laser_max_range",   laser_max_range_ );
           get_parameter(  "laser_min_range",   laser_min_range_ );
           get_parameter(  "laser_model_type",   sensor_model_type_ );
           get_parameter(  "set_initial_pose",   set_initial_pose_ );
           get_parameter(  "initial_pose.x",   initial_pose_x_ );
           get_parameter(  "initial_pose.y",   initial_pose_y_ );
           get_parameter(  "initial_pose.z",   initial_pose_z_ );
           get_parameter(  "initial_pose.yaw",   initial_pose_yaw_ );
           get_parameter(  "max_beams",   max_beams_ );
           get_parameter(  "max_particles",   max_particles_ );
           get_parameter(  "min_particles",   min_particles_ );
           get_parameter(  "odom_frame_id",   odom_frame_id_ );
           get_parameter(  "pf_err",   pf_err_ );
           get_parameter(  "pf_z",   pf_z_ );
           get_parameter(  "recovery_alpha_fast",   alpha_fast_ );
           get_parameter(  "recovery_alpha_slow",   alpha_slow_ );
           get_parameter(  "resample_interval",   resample_interval_ );
           get_parameter(  "robot_model_type",   robot_model_type_ );
           get_parameter(  "save_pose_rate",   save_pose_rate );
           get_parameter(  "sigma_hit",   sigma_hit_ );
           get_parameter(  "tf_broadcast",   tf_broadcast_ );
           get_parameter(  "transform_tolerance",   tmp_tol );
           get_parameter(  "update_min_a",   a_thresh_ );
           get_parameter(  "update_min_d",   d_thresh_ );
           get_parameter(  "z_hit",   z_hit_ );
           get_parameter(  "z_max",   z_max_ );
           get_parameter(  "z_rand",   z_rand_ );
           get_parameter(  "z_short",   z_short_ );
           get_parameter(  "first_map_only",   first_map_only_ );
           get_parameter(  "always_reset_initial_pose",   always_reset_initial_pose_ );
           get_parameter(  "scan_topic",   scan_topic_ );
           get_parameter(  "map_topic",   map_topic_ );
          
           save_pose_period_ = tf2::durationFromSec(  1.0 / save_pose_rate );
           transform_tolerance_ = tf2::durationFromSec(  tmp_tol );
          
           odom_frame_id_ = nav2_util::strip_leading_slash(  odom_frame_id_ );
           base_frame_id_ = nav2_util::strip_leading_slash(  base_frame_id_ );
           global_frame_id_ = nav2_util::strip_leading_slash(  global_frame_id_ );
          
           last_time_printed_msg_ = now(   );
          
           // Semantic checks
           if (  laser_likelihood_max_dist_ < 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),   "You've set laser_likelihood_max_dist to be negtive,  "
           " this isn't allowed so it will be set to default value 2.0." );
           laser_likelihood_max_dist_ = 2.0;
           }
           if (  max_particles_ < 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),   "You've set max_particles to be negtive,  "
           " this isn't allowed so it will be set to default value 2000." );
           max_particles_ = 2000;
           }
          
           if (  min_particles_ < 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),   "You've set min_particles to be negtive,  "
           " this isn't allowed so it will be set to default value 500." );
           min_particles_ = 500;
           }
          
           if (  min_particles_ > max_particles_ ) {
           RCLCPP_WARN(  
           get_logger(   ),   "You've set min_particles to be greater than max particles,  "
           " this isn't allowed so max_particles will be set to min_particles." );
           max_particles_ = min_particles_;
           }
          
           if (  resample_interval_ <= 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),   "You've set resample_interval to be zero or negtive,  "
           " this isn't allowed so it will be set to default value to 1." );
           resample_interval_ = 1;
           }
          
           if (  always_reset_initial_pose_ ) {
           initial_pose_is_known_ = false;
           }
          }
          
          /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
          rcl_interfaces::msg::SetParametersResult
    1138  AmclNode::dynamicParametersCallback(  
    1139   std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
           rcl_interfaces::msg::SetParametersResult result;
           double save_pose_rate;
           double tmp_tol;
          
           int max_particles = max_particles_;
           int min_particles = min_particles_;
          
           bool reinit_pf = false;
           bool reinit_odom = false;
           bool reinit_laser = false;
           bool reinit_map = false;
          
           for (  auto parameter : parameters ) {
           const auto & param_type = parameter.get_type(   );
           const auto & param_name = parameter.get_name(   );
          
           if (  param_type == ParameterType::PARAMETER_DOUBLE ) {
           if (  param_name == "alpha1" ) {
           alpha1_ = parameter.as_double(   );
           reinit_odom = true;
           } else if (  param_name == "alpha2" ) {
           alpha2_ = parameter.as_double(   );
           reinit_odom = true;
           } else if (  param_name == "alpha3" ) {
           alpha3_ = parameter.as_double(   );
           reinit_odom = true;
           } else if (  param_name == "alpha4" ) {
           alpha4_ = parameter.as_double(   );
           reinit_odom = true;
           } else if (  param_name == "alpha5" ) {
           alpha5_ = parameter.as_double(   );
           reinit_odom = true;
           } else if (  param_name == "beam_skip_distance" ) {
           beam_skip_distance_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "beam_skip_error_threshold" ) {
           beam_skip_error_threshold_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "beam_skip_threshold" ) {
           beam_skip_threshold_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "lambda_short" ) {
           lambda_short_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "laser_likelihood_max_dist" ) {
           laser_likelihood_max_dist_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "laser_max_range" ) {
           laser_max_range_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "laser_min_range" ) {
           laser_min_range_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "pf_err" ) {
           pf_err_ = parameter.as_double(   );
           reinit_pf = true;
           } else if (  param_name == "pf_z" ) {
           pf_z_ = parameter.as_double(   );
           reinit_pf = true;
           } else if (  param_name == "recovery_alpha_fast" ) {
           alpha_fast_ = parameter.as_double(   );
           reinit_pf = true;
           } else if (  param_name == "recovery_alpha_slow" ) {
           alpha_slow_ = parameter.as_double(   );
           reinit_pf = true;
           } else if (  param_name == "save_pose_rate" ) {
           save_pose_rate = parameter.as_double(   );
           save_pose_period_ = tf2::durationFromSec(  1.0 / save_pose_rate );
           } else if (  param_name == "sigma_hit" ) {
           sigma_hit_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "transform_tolerance" ) {
           tmp_tol = parameter.as_double(   );
           transform_tolerance_ = tf2::durationFromSec(  tmp_tol );
           reinit_laser = true;
           } else if (  param_name == "update_min_a" ) {
           a_thresh_ = parameter.as_double(   );
           } else if (  param_name == "update_min_d" ) {
           d_thresh_ = parameter.as_double(   );
           } else if (  param_name == "z_hit" ) {
           z_hit_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "z_max" ) {
           z_max_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "z_rand" ) {
           z_rand_ = parameter.as_double(   );
           reinit_laser = true;
           } else if (  param_name == "z_short" ) {
           z_short_ = parameter.as_double(   );
           reinit_laser = true;
           }
           } else if (  param_type == ParameterType::PARAMETER_STRING ) {
           if (  param_name == "base_frame_id" ) {
           base_frame_id_ = parameter.as_string(   );
           } else if (  param_name == "global_frame_id" ) {
           global_frame_id_ = parameter.as_string(   );
           } else if (  param_name == "map_topic" ) {
           map_topic_ = parameter.as_string(   );
           reinit_map = true;
           } else if (  param_name == "laser_model_type" ) {
           sensor_model_type_ = parameter.as_string(   );
           reinit_laser = true;
           } else if (  param_name == "odom_frame_id" ) {
           odom_frame_id_ = parameter.as_string(   );
           reinit_laser = true;
           } else if (  param_name == "scan_topic" ) {
           scan_topic_ = parameter.as_string(   );
           reinit_laser = true;
           } else if (  param_name == "robot_model_type" ) {
           robot_model_type_ = parameter.as_string(   );
           reinit_odom = true;
           }
           } else if (  param_type == ParameterType::PARAMETER_BOOL ) {
           if (  param_name == "do_beamskip" ) {
           do_beamskip_ = parameter.as_bool(   );
           reinit_laser = true;
           } else if (  param_name == "tf_broadcast" ) {
           tf_broadcast_ = parameter.as_bool(   );
           } else if (  param_name == "set_initial_pose" ) {
           set_initial_pose_ = parameter.as_bool(   );
           } else if (  param_name == "first_map_only" ) {
           first_map_only_ = parameter.as_bool(   );
           }
           } else if (  param_type == ParameterType::PARAMETER_INTEGER ) {
           if (  param_name == "max_beams" ) {
           max_beams_ = parameter.as_int(   );
           reinit_laser = true;
           } else if (  param_name == "max_particles" ) {
           max_particles_ = parameter.as_int(   );
           reinit_pf = true;
           } else if (  param_name == "min_particles" ) {
           min_particles_ = parameter.as_int(   );
           reinit_pf = true;
           } else if (  param_name == "resample_interval" ) {
           resample_interval_ = parameter.as_int(   );
           }
           }
           }
          
           // Checking if the minimum particles is greater than max_particles_
           if (  min_particles_ > max_particles_ ) {
           RCLCPP_ERROR(  
           this->get_logger(   ),  
           "You've set min_particles to be greater than max particles,  "
           " this isn't allowed." );
           // sticking to the old values
           max_particles_ = max_particles;
           min_particles_ = min_particles;
           result.successful = false;
           return result;
           }
          
           // Re-initialize the particle filter
           if (  reinit_pf ) {
           if (  pf_ != NULL ) {
           pf_free(  pf_ );
           pf_ = NULL;
           }
           initParticleFilter(   );
           }
          
           // Re-initialize the odometry
           if (  reinit_odom ) {
           motion_model_.reset(   );
           initOdometry(   );
           }
          
           // Re-initialize the lasers and it's filters
           if (  reinit_laser ) {
           lasers_.clear(   );
           lasers_update_.clear(   );
           frame_to_laser_.clear(   );
           laser_scan_connection_.disconnect(   );
           laser_scan_sub_.reset(   );
          
           initMessageFilters(   );
           }
          
           // Re-initialize the map
           if (  reinit_map ) {
           map_sub_.reset(   );
           map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(  
           map_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &AmclNode::mapReceived,   this,   std::placeholders::_1 ) );
           }
          
           result.successful = true;
           return result;
          }
          
          void
    1334  AmclNode::mapReceived(  const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "AmclNode: A new map was received." );
           if (  first_map_only_ && first_map_received_ ) {
           return;
           }
           handleMapMessage(  *msg );
           first_map_received_ = true;
          }
          
          void
    1345  AmclNode::handleMapMessage(  const nav_msgs::msg::OccupancyGrid & msg )
          {
           std::lock_guard<std::recursive_mutex> cfl(  mutex_ );
          
           RCLCPP_INFO(  
           get_logger(   ),   "Received a %d X %d map @ %.3f m/pix",  
           msg.info.width,  
           msg.info.height,  
           msg.info.resolution );
           if (  msg.header.frame_id != global_frame_id_ ) {
           RCLCPP_WARN(  
           get_logger(   ),   "Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could"
           " cause issues with reading published topics",  
           msg.header.frame_id.c_str(   ),  
           global_frame_id_.c_str(   ) );
           }
           freeMapDependentMemory(   );
           map_ = convertMap(  msg );
          
          #if NEW_UNIFORM_SAMPLING
           createFreeSpaceVector(   );
          #endif
          }
          
          void
    1370  AmclNode::createFreeSpaceVector(   )
          {
           // Index of free space
           free_space_indices.resize(  0 );
           for (  int i = 0; i < map_->size_x; i++ ) {
           for (  int j = 0; j < map_->size_y; j++ ) {
           if (  map_->cells[MAP_INDEX(  map_,   i,   j )].occ_state == -1 ) {
           free_space_indices.push_back(  std::make_pair(  i,   j ) );
           }
           }
           }
          }
          
          void
    1384  AmclNode::freeMapDependentMemory(   )
          {
           if (  map_ != NULL ) {
           map_free(  map_ );
           map_ = NULL;
           }
          
           // Clear queued laser objects because they hold pointers to the existing
           // map,   #5202.
           lasers_.clear(   );
           lasers_update_.clear(   );
           frame_to_laser_.clear(   );
          }
          
          // Convert an OccupancyGrid map message into the internal representation. This function
          // allocates a map_t and returns it.
          map_t *
    1401  AmclNode::convertMap(  const nav_msgs::msg::OccupancyGrid & map_msg )
          {
           map_t * map = map_alloc(   );
          
           map->size_x = map_msg.info.width;
           map->size_y = map_msg.info.height;
           map->scale = map_msg.info.resolution;
           map->origin_x = map_msg.info.origin.position.x + (  map->size_x / 2 ) * map->scale;
           map->origin_y = map_msg.info.origin.position.y + (  map->size_y / 2 ) * map->scale;
          
           map->cells =
           reinterpret_cast<map_cell_t *>(  malloc(  sizeof(  map_cell_t ) * map->size_x * map->size_y ) );
          
           // Convert to player format
           for (  int i = 0; i < map->size_x * map->size_y; i++ ) {
           if (  map_msg.data[i] == 0 ) {
           map->cells[i].occ_state = -1;
           } else if (  map_msg.data[i] == 100 ) {
           map->cells[i].occ_state = +1;
           } else {
           map->cells[i].occ_state = 0;
           }
           }
          
           return map;
          }
          
          void
    1429  AmclNode::initTransforms(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "initTransforms" );
          
           // Initialize transform listener and broadcaster
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),  
           get_node_timers_interface(   ),  
           callback_group_ );
           tf_buffer_->setCreateTimerInterface(  timer_interface );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
           tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(  shared_from_this(   ) );
          
           sent_first_transform_ = false;
           latest_tf_valid_ = false;
           latest_tf_ = tf2::Transform::getIdentity(   );
          }
          
          void
    1449  AmclNode::initMessageFilters(   )
          {
           auto sub_opt = rclcpp::SubscriptionOptions(   );
           sub_opt.callback_group = callback_group_;
           laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan,  
           rclcpp_lifecycle::LifecycleNode>>(  
           shared_from_this(   ),   scan_topic_,   rmw_qos_profile_sensor_data,   sub_opt );
          
           laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(  
           *laser_scan_sub_,   *tf_buffer_,   odom_frame_id_,   10,  
           get_node_logging_interface(   ),  
           get_node_clock_interface(   ),  
           transform_tolerance_ );
          
          
           laser_scan_connection_ = laser_scan_filter_->registerCallback(  
           std::bind(  
           &AmclNode::laserReceived,  
           this,   std::placeholders::_1 ) );
          }
          
          void
    1471  AmclNode::initPubSub(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "initPubSub" );
          
           particle_cloud_pub_ = create_publisher<nav2_msgs::msg::ParticleCloud>(  
           "particle_cloud",  
           rclcpp::SensorDataQoS(   ) );
          
           pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
          
           initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "initialpose",   rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &AmclNode::initialPoseReceived,   this,   std::placeholders::_1 ) );
          
           map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(  
           map_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &AmclNode::mapReceived,   this,   std::placeholders::_1 ) );
          
           RCLCPP_INFO(  get_logger(   ),   "Subscribed to map topic." );
          }
          
          void
    1495  AmclNode::initServices(   )
          {
           global_loc_srv_ = create_service<std_srvs::srv::Empty>(  
           "reinitialize_global_localization",  
           std::bind(  &AmclNode::globalLocalizationCallback,   this,   _1,   _2,   _3 ) );
          
           nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(  
           "request_nomotion_update",  
           std::bind(  &AmclNode::nomotionUpdateCallback,   this,   _1,   _2,   _3 ) );
          }
          
          void
    1507  AmclNode::initOdometry(   )
          {
           // TODO(  mjeronimo ): We should handle persistance of the last known pose of the robot. We could
           // then read that pose here and initialize using that.
          
           // When pausing and resuming,   remember the last robot pose so we don't start at 0:0 again
           init_pose_[0] = last_published_pose_.pose.pose.position.x;
           init_pose_[1] = last_published_pose_.pose.pose.position.y;
           init_pose_[2] = tf2::getYaw(  last_published_pose_.pose.pose.orientation );
          
           if (  !initial_pose_is_known_ ) {
           init_cov_[0] = 0.5 * 0.5;
           init_cov_[1] = 0.5 * 0.5;
           init_cov_[2] = (  M_PI / 12.0 ) * (  M_PI / 12.0 );
           } else {
           init_cov_[0] = last_published_pose_.pose.covariance[0];
           init_cov_[1] = last_published_pose_.pose.covariance[7];
           init_cov_[2] = last_published_pose_.pose.covariance[35];
           }
          
           motion_model_ = plugin_loader_.createSharedInstance(  robot_model_type_ );
           motion_model_->initialize(  alpha1_,   alpha2_,   alpha3_,   alpha4_,   alpha5_ );
          
           latest_odom_pose_ = geometry_msgs::msg::PoseStamped(   );
          }
          
          void
    1534  AmclNode::initParticleFilter(   )
          {
           // Create the particle filter
           pf_ = pf_alloc(  
           min_particles_,   max_particles_,   alpha_slow_,   alpha_fast_,  
           (  pf_init_model_fn_t )AmclNode::uniformPoseGenerator,  
           reinterpret_cast<void *>(  map_ ) );
           pf_->pop_err = pf_err_;
           pf_->pop_z = pf_z_;
          
           // Initialize the filter
           pf_vector_t pf_init_pose_mean = pf_vector_zero(   );
           pf_init_pose_mean.v[0] = init_pose_[0];
           pf_init_pose_mean.v[1] = init_pose_[1];
           pf_init_pose_mean.v[2] = init_pose_[2];
          
           pf_matrix_t pf_init_pose_cov = pf_matrix_zero(   );
           pf_init_pose_cov.m[0][0] = init_cov_[0];
           pf_init_pose_cov.m[1][1] = init_cov_[1];
           pf_init_pose_cov.m[2][2] = init_cov_[2];
          
           pf_init(  pf_,   pf_init_pose_mean,   pf_init_pose_cov );
          
           pf_init_ = false;
           resample_count_ = 0;
           memset(  &pf_odom_pose_,   0,   sizeof(  pf_odom_pose_ ) );
          }
          
          void
    1563  AmclNode::initLaserScan(   )
          {
           scan_error_count_ = 0;
           last_laser_received_ts_ = rclcpp::Time(  0 );
          }
          
          } // namespace nav2_amcl
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
          RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_amcl::AmclNode )

navigation2/nav2_amcl/src/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // This library is free software; you can redistribute it and/or
          // modify it under the terms of the GNU Lesser General Public
          // License as published by the Free Software Foundation; either
          // version 2.1 of the License,   or (  at your option ) any later version.
          //
          // This library is distributed in the hope that it will be useful,  
          // but WITHOUT ANY WARRANTY; without even the implied warranty of
          // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
          // Lesser General Public License for more details.
          //
          // You should have received a copy of the GNU Lesser General Public
          // License along with this library; if not,   write to the Free Software
          // Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
          
          #include <memory>
          
          #include "nav2_amcl/amcl_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      22  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_amcl::AmclNode>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_amcl/src/map/map.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Global map (  grid-based )
           * Author: Andrew Howard
           * Date: 6 Feb 2003
           * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
          **************************************************************************/
          
          #include <assert.h>
          #include <math.h>
          #include <stdlib.h>
          #include <string.h>
          #include <stdio.h>
          
          #include "nav2_amcl/map/map.hpp"
          
          
          // Create a new map
      38  map_t * map_alloc(  void )
          {
           map_t * map;
          
           map = (  map_t * ) malloc(  sizeof(  map_t ) );
          
           // Assume we start at (  0,   0 )
           map->origin_x = 0;
           map->origin_y = 0;
          
           // Make the size odd
           map->size_x = 0;
           map->size_y = 0;
           map->scale = 0;
          
           // Allocate storage for main map
           map->cells = (  map_cell_t * ) NULL;
          
           return map;
          }
          
          
          // Destroy a map
      61  void map_free(  map_t * map )
          {
           free(  map->cells );
           free(  map );
          }

navigation2/nav2_amcl/src/map/map_cspace.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include <math.h>
          #include <stdlib.h>
          #include <string.h>
          #include <queue>
          #include "nav2_amcl/map/map.hpp"
          
          /*
           * @class CellData
           * @brief Data about map cells
           */
      32  class CellData
          {
          public:
      35   map_t * map_;
           unsigned int i_,   j_;
           unsigned int src_i_,   src_j_;
          };
          
          /*
           * @class CachedDistanceMap
           * @brief Cached map with distances
           */
      44  class CachedDistanceMap
          {
          public:
           /*
           * @brief CachedDistanceMap constructor
           */
      50   CachedDistanceMap(  double scale,   double max_dist )
           : distances_(  NULL ),   scale_(  scale ),   max_dist_(  max_dist )
           {
           cell_radius_ = max_dist / scale;
           distances_ = new double *[cell_radius_ + 2];
           for (  int i = 0; i <= cell_radius_ + 1; i++ ) {
           distances_[i] = new double[cell_radius_ + 2];
           for (  int j = 0; j <= cell_radius_ + 1; j++ ) {
           distances_[i][j] = sqrt(  i * i + j * j );
           }
           }
           }
          
           /*
           * @brief CachedDistanceMap destructor
           */
      66   ~CachedDistanceMap(   )
           {
           if (  distances_ ) {
           for (  int i = 0; i <= cell_radius_ + 1; i++ ) {
           delete[] distances_[i];
           }
           delete[] distances_;
           }
           }
           double ** distances_;
           double scale_;
           double max_dist_;
           int cell_radius_;
          };
          
          /*
           * @brief operator<
           */
      84  bool operator<(  const CellData & a,   const CellData & b )
          {
           return a.map_->cells[MAP_INDEX(  
           a.map_,   a.i_,  
           a.j_ )].occ_dist > a.map_->cells[MAP_INDEX(  b.map_,   b.i_,   b.j_ )].occ_dist;
          }
          
          /*
           * @brief get_distance_map
           * @param scale of cost information wrt distance
           * @param max_dist Maximum distance to cache from occupied information
           * @return Pointer to cached distance map
           */
          CachedDistanceMap *
      98  get_distance_map(  double scale,   double max_dist )
          {
           static CachedDistanceMap * cdm = NULL;
          
           if (  !cdm || (  cdm->scale_ != scale ) || (  cdm->max_dist_ != max_dist ) ) {
           if (  cdm ) {
           delete cdm;
           }
           cdm = new CachedDistanceMap(  scale,   max_dist );
           }
          
           return cdm;
          }
          
          /*
           * @brief enqueue cell data for caching
           */
     115  void enqueue(  
           map_t * map,   int i,   int j,  
           int src_i,   int src_j,  
           std::priority_queue<CellData> & Q,  
           CachedDistanceMap * cdm,  
           unsigned char * marked )
          {
           if (  marked[MAP_INDEX(  map,   i,   j )] ) {
           return;
           }
          
           int di = abs(  i - src_i );
           int dj = abs(  j - src_j );
           double distance = cdm->distances_[di][dj];
          
           if (  distance > cdm->cell_radius_ ) {
           return;
           }
          
           map->cells[MAP_INDEX(  map,   i,   j )].occ_dist = distance * map->scale;
          
           CellData cell;
           cell.map_ = map;
           cell.i_ = i;
           cell.j_ = j;
           cell.src_i_ = src_i;
           cell.src_j_ = src_j;
          
           Q.push(  cell );
          
           marked[MAP_INDEX(  map,   i,   j )] = 1;
          }
          
          /*
           * @brief Update the cspace distance values
           * @param map Map to update
           * @param max_occ_distance Maximum distance for occpuancy interest
           */
     153  void map_update_cspace(  map_t * map,   double max_occ_dist )
          {
           unsigned char * marked;
           std::priority_queue<CellData> Q;
          
           marked = new unsigned char[map->size_x * map->size_y];
           memset(  marked,   0,   sizeof(  unsigned char ) * map->size_x * map->size_y );
          
           map->max_occ_dist = max_occ_dist;
          
           CachedDistanceMap * cdm = get_distance_map(  map->scale,   map->max_occ_dist );
          
           // Enqueue all the obstacle cells
           CellData cell;
           cell.map_ = map;
           for (  int i = 0; i < map->size_x; i++ ) {
           cell.src_i_ = cell.i_ = i;
           for (  int j = 0; j < map->size_y; j++ ) {
           if (  map->cells[MAP_INDEX(  map,   i,   j )].occ_state == +1 ) {
           map->cells[MAP_INDEX(  map,   i,   j )].occ_dist = 0.0;
           cell.src_j_ = cell.j_ = j;
           marked[MAP_INDEX(  map,   i,   j )] = 1;
           Q.push(  cell );
           } else {
           map->cells[MAP_INDEX(  map,   i,   j )].occ_dist = max_occ_dist;
           }
           }
           }
          
           while (  !Q.empty(   ) ) {
           CellData current_cell = Q.top(   );
           if (  current_cell.i_ > 0 ) {
           enqueue(  
           map,   current_cell.i_ - 1,   current_cell.j_,  
           current_cell.src_i_,   current_cell.src_j_,  
           Q,   cdm,   marked );
           }
           if (  current_cell.j_ > 0 ) {
           enqueue(  
           map,   current_cell.i_,   current_cell.j_ - 1,  
           current_cell.src_i_,   current_cell.src_j_,  
           Q,   cdm,   marked );
           }
           if (  static_cast<int>(  current_cell.i_ ) < map->size_x - 1 ) {
           enqueue(  
           map,   current_cell.i_ + 1,   current_cell.j_,  
           current_cell.src_i_,   current_cell.src_j_,  
           Q,   cdm,   marked );
           }
           if (  static_cast<int>(  current_cell.j_ ) < map->size_y - 1 ) {
           enqueue(  
           map,   current_cell.i_,   current_cell.j_ + 1,  
           current_cell.src_i_,   current_cell.src_j_,  
           Q,   cdm,   marked );
           }
          
           Q.pop(   );
           }
          
           delete[] marked;
          }

navigation2/nav2_amcl/src/map/map_draw.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Local map GUI functions
           * Author: Andrew Howard
           * Date: 18 Jan 2003
           * CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
          **************************************************************************/
          
          #pragma GCC diagnostic ignored "-Wpedantic"
          #ifdef INCLUDE_RTKGUI
          
          #include <errno.h>
          #include <math.h>
          #include <stdlib.h>
          #include <string.h>
          
          #include <rtk.h>
          #include "nav2_amcl/map/map.hpp"
          
          
          ////////////////////////////////////////////////////////////////////////////
          // Draw the occupancy map
      42  void map_draw_occ(  map_t * map,   rtk_fig_t * fig )
          {
           int i,   j;
           int col;
           map_cell_t * cell;
           uint16_t * image;
           uint16_t * pixel;
          
           image = malloc(  map->size_x * map->size_y * sizeof(  image[0] ) );
          
           // Draw occupancy
           for (  j = 0; j < map->size_y; j++ ) {
           for (  i = 0; i < map->size_x; i++ ) {
           cell = map->cells + MAP_INDEX(  map,   i,   j );
           pixel = image + (  j * map->size_x + i );
          
           col = 127 - 127 * cell->occ_state;
           *pixel = RTK_RGB16(  col,   col,   col );
           }
           }
          
           // Draw the entire occupancy map as an image
           rtk_fig_image(  
           fig,   map->origin_x,   map->origin_y,   0,  
           map->scale,   map->size_x,   map->size_y,   16,   image,   NULL );
          
           free(  image );
          }
          
          
          ////////////////////////////////////////////////////////////////////////////
          // Draw the cspace map
      74  void map_draw_cspace(  map_t * map,   rtk_fig_t * fig )
          {
           int i,   j;
           int col;
           map_cell_t * cell;
           uint16_t * image;
           uint16_t * pixel;
          
           image = malloc(  map->size_x * map->size_y * sizeof(  image[0] ) );
          
           // Draw occupancy
           for (  j = 0; j < map->size_y; j++ ) {
           for (  i = 0; i < map->size_x; i++ ) {
           cell = map->cells + MAP_INDEX(  map,   i,   j );
           pixel = image + (  j * map->size_x + i );
          
           col = 255 * cell->occ_dist / map->max_occ_dist;
          
           *pixel = RTK_RGB16(  col,   col,   col );
           }
           }
          
           // Draw the entire occupancy map as an image
           rtk_fig_image(  
           fig,   map->origin_x,   map->origin_y,   0,  
           map->scale,   map->size_x,   map->size_y,   16,   image,   NULL );
          
           free(  image );
          }
          
          
          ////////////////////////////////////////////////////////////////////////////
          // Draw a wifi map
     107  void map_draw_wifi(  map_t * map,   rtk_fig_t * fig,   int index )
          {
           int i,   j;
           int level,   col;
           map_cell_t * cell;
           uint16_t * image,   * mask;
           uint16_t * ipix,   * mpix;
          
           image = malloc(  map->size_x * map->size_y * sizeof(  image[0] ) );
           mask = malloc(  map->size_x * map->size_y * sizeof(  mask[0] ) );
          
           // Draw wifi levels
           for (  j = 0; j < map->size_y; j++ ) {
           for (  i = 0; i < map->size_x; i++ ) {
           cell = map->cells + MAP_INDEX(  map,   i,   j );
           ipix = image + (  j * map->size_x + i );
           mpix = mask + (  j * map->size_x + i );
          
           level = cell->wifi_levels[index];
          
           if (  cell->occ_state == -1 && level != 0 ) {
           col = 255 * (  100 + level ) / 100;
           *ipix = RTK_RGB16(  col,   col,   col );
           *mpix = 1;
           } else {
           *mpix = 0;
           }
           }
           }
          
           // Draw the entire occupancy map as an image
           rtk_fig_image(  
           fig,   map->origin_x,   map->origin_y,   0,  
           map->scale,   map->size_x,   map->size_y,   16,   image,   mask );
          
           free(  mask );
           free(  image );
          }
          
          
          #endif

navigation2/nav2_amcl/src/map/map_range.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Range routines
           * Author: Andrew Howard
           * Date: 18 Jan 2003
           * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
          **************************************************************************/
          
          #include <assert.h>
          #include <math.h>
          #include <string.h>
          #include <stdlib.h>
          
          #include "nav2_amcl/map/map.hpp"
          
          // Extract a single range reading from the map. Unknown cells and/or
          // out-of-bound cells are treated as occupied,   which makes it easy to
          // use Stage bitmap files.
      38  double map_calc_range(  map_t * map,   double ox,   double oy,   double oa,   double max_range )
          {
           // Bresenham raytracing
           int x0,   x1,   y0,   y1;
           int x,   y;
           int xstep,   ystep;
           char steep;
           int tmp;
           int deltax,   deltay,   error,   deltaerr;
          
           x0 = MAP_GXWX(  map,   ox );
           y0 = MAP_GYWY(  map,   oy );
          
           x1 = MAP_GXWX(  map,   ox + max_range * cos(  oa ) );
           y1 = MAP_GYWY(  map,   oy + max_range * sin(  oa ) );
          
           if (  abs(  y1 - y0 ) > abs(  x1 - x0 ) ) {
           steep = 1;
           } else {
           steep = 0;
           }
          
           if (  steep ) {
           tmp = x0;
           x0 = y0;
           y0 = tmp;
          
           tmp = x1;
           x1 = y1;
           y1 = tmp;
           }
          
           deltax = abs(  x1 - x0 );
           deltay = abs(  y1 - y0 );
           error = 0;
           deltaerr = deltay;
          
           x = x0;
           y = y0;
          
           if (  x0 < x1 ) {
           xstep = 1;
           } else {
           xstep = -1;
           }
           if (  y0 < y1 ) {
           ystep = 1;
           } else {
           ystep = -1;
           }
          
           if (  steep ) {
           if (  !MAP_VALID(  map,   y,   x ) || map->cells[MAP_INDEX(  map,   y,   x )].occ_state > -1 ) {
           return sqrt(  (  x - x0 ) * (  x - x0 ) + (  y - y0 ) * (  y - y0 ) ) * map->scale;
           }
           } else {
           if (  !MAP_VALID(  map,   x,   y ) || map->cells[MAP_INDEX(  map,   x,   y )].occ_state > -1 ) {
           return sqrt(  (  x - x0 ) * (  x - x0 ) + (  y - y0 ) * (  y - y0 ) ) * map->scale;
           }
           }
          
           while (  x != (  x1 + xstep * 1 ) ) {
           x += xstep;
           error += deltaerr;
           if (  2 * error >= deltax ) {
           y += ystep;
           error -= deltax;
           }
          
           if (  steep ) {
           if (  !MAP_VALID(  map,   y,   x ) || map->cells[MAP_INDEX(  map,   y,   x )].occ_state > -1 ) {
           return sqrt(  (  x - x0 ) * (  x - x0 ) + (  y - y0 ) * (  y - y0 ) ) * map->scale;
           }
           } else {
           if (  !MAP_VALID(  map,   x,   y ) || map->cells[MAP_INDEX(  map,   x,   y )].occ_state > -1 ) {
           return sqrt(  (  x - x0 ) * (  x - x0 ) + (  y - y0 ) * (  y - y0 ) ) * map->scale;
           }
           }
           }
           return max_range;
          }

navigation2/nav2_amcl/src/motion_model/differential_motion_model.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include "nav2_amcl/motion_model/differential_motion_model.hpp"
          
          namespace nav2_amcl
          {
          
          void
      28  DifferentialMotionModel::initialize(  
           double alpha1,   double alpha2,   double alpha3,   double alpha4,  
           double alpha5 )
          {
           alpha1_ = alpha1;
           alpha2_ = alpha2;
           alpha3_ = alpha3;
           alpha4_ = alpha4;
           alpha5_ = alpha5;
          }
          
          void
      40  DifferentialMotionModel::odometryUpdate(  
      41   pf_t * pf,   const pf_vector_t & pose,  
      42   const pf_vector_t & delta )
          {
           // Compute the new sample poses
           pf_sample_set_t * set;
          
           set = pf->sets + pf->current_set;
           pf_vector_t old_pose = pf_vector_sub(  pose,   delta );
          
           // Implement sample_motion_odometry (  Prob Rob p 136 )
           double delta_rot1,   delta_trans,   delta_rot2;
           double delta_rot1_hat,   delta_trans_hat,   delta_rot2_hat;
           double delta_rot1_noise,   delta_rot2_noise;
          
           // Avoid computing a bearing from two poses that are extremely near each
           // other (  happens on in-place rotation ).
           if (  sqrt(  
           delta.v[1] * delta.v[1] +
           delta.v[0] * delta.v[0] ) < 0.01 )
           {
           delta_rot1 = 0.0;
           } else {
           delta_rot1 = angleutils::angle_diff(  
           atan2(  delta.v[1],   delta.v[0] ),  
           old_pose.v[2] );
           }
           delta_trans = sqrt(  
           delta.v[0] * delta.v[0] +
           delta.v[1] * delta.v[1] );
           delta_rot2 = angleutils::angle_diff(  delta.v[2],   delta_rot1 );
          
           // We want to treat backward and forward motion symmetrically for the
           // noise model to be applied below. The standard model seems to assume
           // forward motion.
           delta_rot1_noise = std::min(  
           fabs(  angleutils::angle_diff(  delta_rot1,   0.0 ) ),  
           fabs(  angleutils::angle_diff(  delta_rot1,   M_PI ) ) );
           delta_rot2_noise = std::min(  
           fabs(  angleutils::angle_diff(  delta_rot2,   0.0 ) ),  
           fabs(  angleutils::angle_diff(  delta_rot2,   M_PI ) ) );
          
           for (  int i = 0; i < set->sample_count; i++ ) {
           pf_sample_t * sample = set->samples + i;
          
           // Sample pose differences
           delta_rot1_hat = angleutils::angle_diff(  
           delta_rot1,  
           pf_ran_gaussian(  
           sqrt(  
           alpha1_ * delta_rot1_noise * delta_rot1_noise +
           alpha2_ * delta_trans * delta_trans ) ) );
           delta_trans_hat = delta_trans -
           pf_ran_gaussian(  
           sqrt(  
           alpha3_ * delta_trans * delta_trans +
           alpha4_ * delta_rot1_noise * delta_rot1_noise +
           alpha4_ * delta_rot2_noise * delta_rot2_noise ) );
           delta_rot2_hat = angleutils::angle_diff(  
           delta_rot2,  
           pf_ran_gaussian(  
           sqrt(  
           alpha1_ * delta_rot2_noise * delta_rot2_noise +
           alpha2_ * delta_trans * delta_trans ) ) );
          
           // Apply sampled update to particle pose
           sample->pose.v[0] += delta_trans_hat *
           cos(  sample->pose.v[2] + delta_rot1_hat );
           sample->pose.v[1] += delta_trans_hat *
           sin(  sample->pose.v[2] + delta_rot1_hat );
           sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
           }
          }
          
          } // namespace nav2_amcl
          
          #include <pluginlib/class_list_macros.hpp>
     117  PLUGINLIB_EXPORT_CLASS(  nav2_amcl::DifferentialMotionModel,   nav2_amcl::MotionModel )

navigation2/nav2_amcl/src/motion_model/omni_motion_model.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include "nav2_amcl/motion_model/omni_motion_model.hpp"
          
          namespace nav2_amcl
          {
          
          void
      28  OmniMotionModel::initialize(  
           double alpha1,   double alpha2,   double alpha3,   double alpha4,  
           double alpha5 )
          {
           alpha1_ = alpha1;
           alpha2_ = alpha2;
           alpha3_ = alpha3;
           alpha4_ = alpha4;
           alpha5_ = alpha5;
          }
          
          void
      40  OmniMotionModel::odometryUpdate(  
      41   pf_t * pf,   const pf_vector_t & pose,  
      42   const pf_vector_t & delta )
          {
           // Compute the new sample poses
           pf_sample_set_t * set;
          
           set = pf->sets + pf->current_set;
           pf_vector_t old_pose = pf_vector_sub(  pose,   delta );
          
           double delta_trans,   delta_rot,   delta_bearing;
           double delta_trans_hat,   delta_rot_hat,   delta_strafe_hat;
          
           delta_trans = sqrt(  
           delta.v[0] * delta.v[0] +
           delta.v[1] * delta.v[1] );
           delta_rot = delta.v[2];
          
           // Precompute a couple of things
           double trans_hat_stddev = sqrt(  
           alpha3_ * (  delta_trans * delta_trans ) +
           alpha4_ * (  delta_rot * delta_rot )  );
           double rot_hat_stddev = sqrt(  
           alpha1_ * (  delta_rot * delta_rot ) +
           alpha2_ * (  delta_trans * delta_trans )  );
           double strafe_hat_stddev = sqrt(  
           alpha4_ * (  delta_rot * delta_rot ) +
           alpha5_ * (  delta_trans * delta_trans )  );
          
           for (  int i = 0; i < set->sample_count; i++ ) {
           pf_sample_t * sample = set->samples + i;
          
           delta_bearing = angleutils::angle_diff(  
           atan2(  delta.v[1],   delta.v[0] ),  
           old_pose.v[2] ) + sample->pose.v[2];
           double cs_bearing = cos(  delta_bearing );
           double sn_bearing = sin(  delta_bearing );
          
           // Sample pose differences
           delta_trans_hat = delta_trans + pf_ran_gaussian(  trans_hat_stddev );
           delta_rot_hat = delta_rot + pf_ran_gaussian(  rot_hat_stddev );
           delta_strafe_hat = 0 + pf_ran_gaussian(  strafe_hat_stddev );
           // Apply sampled update to particle pose
           sample->pose.v[0] += (  delta_trans_hat * cs_bearing +
           delta_strafe_hat * sn_bearing );
           sample->pose.v[1] += (  delta_trans_hat * sn_bearing -
           delta_strafe_hat * cs_bearing );
           sample->pose.v[2] += delta_rot_hat;
           }
          }
          
          } // namespace nav2_amcl
          
          #include <pluginlib/class_list_macros.hpp>
      94  PLUGINLIB_EXPORT_CLASS(  nav2_amcl::OmniMotionModel,   nav2_amcl::MotionModel )

navigation2/nav2_amcl/src/pf/eig3.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /* Eigen decomposition code for symmetric 3x3 matrices,   copied from the public
           domain Java Matrix library JAMA. */
          
          #include <math.h>
          
          #ifndef MAX
          #define MAX(  a,   b ) (  (  a ) > (  b ) ? (  a ) : (  b ) )
          #endif
          
          #ifdef _MSC_VER
          #define n 3
          #else
          static int n = 3;
          #endif
          
          // Symmetric Householder reduction to tridiagonal form.
          
      38  static void tred2(  double V[n][n],   double d[n],   double e[n] )
          {
          // This is derived from the Algol procedures tred2 by
          // Bowdler,   Martin,   Reinsch,   and Wilkinson,   Handbook for
          // Auto. Comp.,   Vol.ii-Linear Algebra,   and the corresponding
          // Fortran subroutine in EISPACK.
          
           int i,   j,   k;
           double f,   g,   h,   hh;
           for (  j = 0; j < n; j++ ) {
           d[j] = V[n - 1][j];
           }
          
           // Householder reduction to tridiagonal form.
          
           for (  i = n - 1; i > 0; i-- ) {
           // Scale to avoid under/overflow.
          
           double scale = 0.0;
           double h = 0.0;
           for (  k = 0; k < i; k++ ) {
           scale = scale + fabs(  d[k] );
           }
           if (  scale == 0.0 ) {
           e[i] = d[i - 1];
           for (  j = 0; j < i; j++ ) {
           d[j] = V[i - 1][j];
           V[i][j] = 0.0;
           V[j][i] = 0.0;
           }
           } else {
           // Generate Householder vector.
           for (  k = 0; k < i; k++ ) {
           d[k] /= scale;
           h += d[k] * d[k];
           }
           f = d[i - 1];
           g = sqrt(  h );
           if (  f > 0 ) {
           g = -g;
           }
           e[i] = scale * g;
           h = h - f * g;
           d[i - 1] = f - g;
           for (  j = 0; j < i; j++ ) {
           e[j] = 0.0;
           }
          
           // Apply similarity transformation to remaining columns.
          
           for (  j = 0; j < i; j++ ) {
           f = d[j];
           V[j][i] = f;
           g = e[j] + V[j][j] * f;
           for (  k = j + 1; k <= i - 1; k++ ) {
           g += V[k][j] * d[k];
           e[k] += V[k][j] * f;
           }
           e[j] = g;
           }
           f = 0.0;
           for (  j = 0; j < i; j++ ) {
           e[j] /= h;
           f += e[j] * d[j];
           }
           hh = f / (  h + h );
           for (  j = 0; j < i; j++ ) {
           e[j] -= hh * d[j];
           }
           for (  j = 0; j < i; j++ ) {
           f = d[j];
           g = e[j];
           for (  k = j; k <= i - 1; k++ ) {
           V[k][j] -= (  f * e[k] + g * d[k] );
           }
           d[j] = V[i - 1][j];
           V[i][j] = 0.0;
           }
           }
           d[i] = h;
           }
          
           // Accumulate transformations.
          
           for (  i = 0; i < n - 1; i++ ) {
           V[n - 1][i] = V[i][i];
           V[i][i] = 1.0;
           h = d[i + 1];
           if (  h != 0.0 ) {
           for (  k = 0; k <= i; k++ ) {
           d[k] = V[k][i + 1] / h;
           }
           for (  j = 0; j <= i; j++ ) {
           g = 0.0;
           for (  k = 0; k <= i; k++ ) {
           g += V[k][i + 1] * V[k][j];
           }
           for (  k = 0; k <= i; k++ ) {
           V[k][j] -= g * d[k];
           }
           }
           }
           for (  k = 0; k <= i; k++ ) {
           V[k][i + 1] = 0.0;
           }
           }
           for (  j = 0; j < n; j++ ) {
           d[j] = V[n - 1][j];
           V[n - 1][j] = 0.0;
           }
           V[n - 1][n - 1] = 1.0;
           e[0] = 0.0;
          }
          
          // Symmetric tridiagonal QL algorithm.
          
     154  static void tql2(  double V[n][n],   double d[n],   double e[n] )
          {
          // This is derived from the Algol procedures tql2,   by
          // Bowdler,   Martin,   Reinsch,   and Wilkinson,   Handbook for
          // Auto. Comp.,   Vol.ii-Linear Algebra,   and the corresponding
          // Fortran subroutine in EISPACK.
           int i,   j,   m,   l,   k;
           double g,   p,   r,   dl1,   h,   f,   tst1,   eps;
           double c,   c2,   c3,   el1,   s,   s2;
          
           for (  i = 1; i < n; i++ ) {
           e[i - 1] = e[i];
           }
           e[n - 1] = 0.0;
          
           f = 0.0;
           tst1 = 0.0;
           eps = pow(  2.0,   -52.0 );
           for (  l = 0; l < n; l++ ) {
           // Find small subdiagonal element
           tst1 = MAX(  tst1,   fabs(  d[l] ) + fabs(  e[l] ) );
           m = l;
           while (  m < n ) {
           if (  fabs(  e[m] ) <= eps * tst1 ) {
           break;
           }
           m++;
           }
          
           // If m == l,   d[l] is an eigenvalue,  
           // otherwise,   iterate.
          
           if (  m > l ) {
           int iter = 0;
           do {
           iter = iter + 1; // (  Could check iteration count here. )
          
           // Compute implicit shift
          
           g = d[l];
           p = (  d[l + 1] - g ) / (  2.0 * e[l] );
           r = hypot(  p,   1.0 );
           if (  p < 0 ) {
           r = -r;
           }
           d[l] = e[l] / (  p + r );
           d[l + 1] = e[l] * (  p + r );
           dl1 = d[l + 1];
           h = g - d[l];
           for (  i = l + 2; i < n; i++ ) {
           d[i] -= h;
           }
           f = f + h;
          
           // Implicit QL transformation.
          
           p = d[m];
           c = 1.0;
           c2 = c;
           c3 = c;
           el1 = e[l + 1];
           s = 0.0;
           s2 = 0.0;
           for (  i = m - 1; i >= l; i-- ) {
           c3 = c2;
           c2 = c;
           s2 = s;
           g = c * e[i];
           h = c * p;
           r = hypot(  p,   e[i] );
           e[i + 1] = s * r;
           s = e[i] / r;
           c = p / r;
           p = c * d[i] - s * g;
           d[i + 1] = h + s * (  c * g + s * d[i] );
          
           // Accumulate transformation.
          
           for (  k = 0; k < n; k++ ) {
           h = V[k][i + 1];
           V[k][i + 1] = s * V[k][i] + c * h;
           V[k][i] = c * V[k][i] - s * h;
           }
           }
           p = -s * s2 * c3 * el1 * e[l] / dl1;
           e[l] = s * p;
           d[l] = c * p;
          
           // Check for convergence.
           } while (  fabs(  e[l] ) > eps * tst1 );
           }
           d[l] = d[l] + f;
           e[l] = 0.0;
           }
           // Sort eigenvalues and corresponding vectors.
          
           for (  i = 0; i < n - 1; i++ ) {
           k = i;
           p = d[i];
           for (  j = i + 1; j < n; j++ ) {
           if (  d[j] < p ) {
           k = j;
           p = d[j];
           }
           }
           if (  k != i ) {
           d[k] = d[i];
           d[i] = p;
           for (  j = 0; j < n; j++ ) {
           p = V[j][i];
           V[j][i] = V[j][k];
           V[j][k] = p;
           }
           }
           }
          }
          
     271  void eigen_decomposition(  double A[n][n],   double V[n][n],   double d[n] )
          {
           int i,   j;
           double e[n]; // NOLINT
           for (  i = 0; i < n; i++ ) {
           for (  j = 0; j < n; j++ ) {
           V[i][j] = A[i][j];
           }
           }
           tred2(  V,   d,   e );
           tql2(  V,   d,   e );
          }

navigation2/nav2_amcl/src/pf/pf.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Simple particle filter for localization.
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
           *************************************************************************/
          
          #include <float.h>
          #include <assert.h>
          #include <math.h>
          #include <stdlib.h>
          #include <time.h>
          
          #include "nav2_amcl/pf/pf.hpp"
          #include "nav2_amcl/pf/pf_pdf.hpp"
          #include "nav2_amcl/pf/pf_kdtree.hpp"
          
          #include "nav2_amcl/portable_utils.hpp"
          
          
          // Compute the required number of samples,   given that there are k bins
          // with samples in them.
      43  static int pf_resample_limit(  pf_t * pf,   int k );
          
          
          // Create a new filter
      47  pf_t * pf_alloc(  
           int min_samples,   int max_samples,  
           double alpha_slow,   double alpha_fast,  
           pf_init_model_fn_t random_pose_fn,   void * random_pose_data )
          {
           int i,   j;
           pf_t * pf;
           pf_sample_set_t * set;
           pf_sample_t * sample;
          
           srand48(  time(  NULL ) );
          
           pf = calloc(  1,   sizeof(  pf_t ) );
          
           pf->random_pose_fn = random_pose_fn;
           pf->random_pose_data = random_pose_data;
          
           pf->min_samples = min_samples;
           pf->max_samples = max_samples;
          
           // Control parameters for the population size calculation. [err] is
           // the max error between the true distribution and the estimated
           // distribution. [z] is the upper standard normal quantile for (  1 -
           // p ),   where p is the probability that the error on the estimated
           // distrubition will be less than [err].
           pf->pop_err = 0.01;
           pf->pop_z = 3;
           pf->dist_threshold = 0.5;
          
           pf->current_set = 0;
           for (  j = 0; j < 2; j++ ) {
           set = pf->sets + j;
          
           set->sample_count = max_samples;
           set->samples = calloc(  max_samples,   sizeof(  pf_sample_t ) );
          
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           sample->pose.v[0] = 0.0;
           sample->pose.v[1] = 0.0;
           sample->pose.v[2] = 0.0;
           sample->weight = 1.0 / max_samples;
           }
          
           // HACK: is 3 times max_samples enough?
           set->kdtree = pf_kdtree_alloc(  3 * max_samples );
          
           set->cluster_count = 0;
           set->cluster_max_count = max_samples;
           set->clusters = calloc(  set->cluster_max_count,   sizeof(  pf_cluster_t ) );
          
           set->mean = pf_vector_zero(   );
           set->cov = pf_matrix_zero(   );
           }
          
           pf->w_slow = 0.0;
           pf->w_fast = 0.0;
          
           pf->alpha_slow = alpha_slow;
           pf->alpha_fast = alpha_fast;
          
           // set converged to 0
           pf_init_converged(  pf );
          
           return pf;
          }
          
          // Free an existing filter
     115  void pf_free(  pf_t * pf )
          {
           int i;
          
           for (  i = 0; i < 2; i++ ) {
           free(  pf->sets[i].clusters );
           pf_kdtree_free(  pf->sets[i].kdtree );
           free(  pf->sets[i].samples );
           }
           free(  pf );
          }
          
          // Initialize the filter using a guassian
     128  void pf_init(  pf_t * pf,   pf_vector_t mean,   pf_matrix_t cov )
          {
           int i;
           pf_sample_set_t * set;
           pf_sample_t * sample;
           pf_pdf_gaussian_t * pdf;
          
           set = pf->sets + pf->current_set;
          
           // Create the kd tree for adaptive sampling
           pf_kdtree_clear(  set->kdtree );
          
           set->sample_count = pf->max_samples;
          
           pdf = pf_pdf_gaussian_alloc(  mean,   cov );
          
           // Compute the new sample poses
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           sample->weight = 1.0 / pf->max_samples;
           sample->pose = pf_pdf_gaussian_sample(  pdf );
          
           // Add sample to histogram
           pf_kdtree_insert(  set->kdtree,   sample->pose,   sample->weight );
           }
          
           pf->w_slow = pf->w_fast = 0.0;
          
           pf_pdf_gaussian_free(  pdf );
          
           // Re-compute cluster statistics
           pf_cluster_stats(  pf,   set );
          
           // set converged to 0
           pf_init_converged(  pf );
          }
          
          
          // Initialize the filter using some model
     167  void pf_init_model(  pf_t * pf,   pf_init_model_fn_t init_fn,   void * init_data )
          {
           int i;
           pf_sample_set_t * set;
           pf_sample_t * sample;
          
           set = pf->sets + pf->current_set;
          
           // Create the kd tree for adaptive sampling
           pf_kdtree_clear(  set->kdtree );
          
           set->sample_count = pf->max_samples;
          
           // Compute the new sample poses
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           sample->weight = 1.0 / pf->max_samples;
           sample->pose = (  *init_fn )(  init_data );
          
           // Add sample to histogram
           pf_kdtree_insert(  set->kdtree,   sample->pose,   sample->weight );
           }
          
           pf->w_slow = pf->w_fast = 0.0;
          
           // Re-compute cluster statistics
           pf_cluster_stats(  pf,   set );
          
           // set converged to 0
           pf_init_converged(  pf );
          }
          
     199  void pf_init_converged(  pf_t * pf )
          {
           pf_sample_set_t * set;
           set = pf->sets + pf->current_set;
           set->converged = 0;
           pf->converged = 0;
          }
          
     207  int pf_update_converged(  pf_t * pf )
          {
           int i;
           pf_sample_set_t * set;
           pf_sample_t * sample;
          
           set = pf->sets + pf->current_set;
           double mean_x = 0,   mean_y = 0;
          
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
          
           mean_x += sample->pose.v[0];
           mean_y += sample->pose.v[1];
           }
           mean_x /= set->sample_count;
           mean_y /= set->sample_count;
          
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           if (  fabs(  sample->pose.v[0] - mean_x ) > pf->dist_threshold ||
           fabs(  sample->pose.v[1] - mean_y ) > pf->dist_threshold )
           {
           set->converged = 0;
           pf->converged = 0;
           return 0;
           }
           }
           set->converged = 1;
           pf->converged = 1;
           return 1;
          }
          
          // Update the filter with some new action
          // void pf_update_action(  pf_t * pf,   pf_action_model_fn_t action_fn,   void * action_data )
          // {
          // pf_sample_set_t * set;
          
          // set = pf->sets + pf->current_set;
          
          // (  *action_fn )(  action_data,   set );
          // }
          
          // Update the filter with some new sensor observation
     251  void pf_update_sensor(  pf_t * pf,   pf_sensor_model_fn_t sensor_fn,   void * sensor_data )
          {
           int i;
           pf_sample_set_t * set;
           pf_sample_t * sample;
           double total;
          
           set = pf->sets + pf->current_set;
          
           // Compute the sample weights
           total = (  *sensor_fn )(  sensor_data,   set );
          
           if (  total > 0.0 ) {
           // Normalize weights
           double w_avg = 0.0;
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           w_avg += sample->weight;
           sample->weight /= total;
           }
           // Update running averages of likelihood of samples (  Prob Rob p258 )
           w_avg /= set->sample_count;
           if (  pf->w_slow == 0.0 ) {
           pf->w_slow = w_avg;
           } else {
           pf->w_slow += pf->alpha_slow * (  w_avg - pf->w_slow );
           }
           if (  pf->w_fast == 0.0 ) {
           pf->w_fast = w_avg;
           } else {
           pf->w_fast += pf->alpha_fast * (  w_avg - pf->w_fast );
           }
           } else {
           // Handle zero total
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
           sample->weight = 1.0 / set->sample_count;
           }
           }
          }
          
          
          // Resample the distribution
     294  void pf_update_resample(  pf_t * pf )
          {
           int i;
           double total;
           pf_sample_set_t * set_a,   * set_b;
           pf_sample_t * sample_a,   * sample_b;
          
           // double r,  c,  U;
           // int m;
           // double count_inv;
           double * c;
          
           double w_diff;
          
           set_a = pf->sets + pf->current_set;
           set_b = pf->sets + (  pf->current_set + 1 ) % 2;
          
           // Build up cumulative probability table for resampling.
           // TODO(  ? ): Replace this with a more efficient procedure
           // (  e.g.,   http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html )
           c = (  double * )malloc(  sizeof(  double ) * (  set_a->sample_count + 1 ) );
           c[0] = 0.0;
           for (  i = 0; i < set_a->sample_count; i++ ) {
           c[i + 1] = c[i] + set_a->samples[i].weight;
           }
          
           // Create the kd tree for adaptive sampling
           pf_kdtree_clear(  set_b->kdtree );
          
           // Draw samples from set a to create set b.
           total = 0;
           set_b->sample_count = 0;
          
           w_diff = 1.0 - pf->w_fast / pf->w_slow;
           if (  w_diff < 0.0 ) {
           w_diff = 0.0;
           }
           // printf(  "w_diff: %9.6f\n",   w_diff );
          
           // Can't (  easily ) combine low-variance sampler with KLD adaptive
           // sampling,   so we'll take the more traditional route.
           /*
           // Low-variance resampler,   taken from Probabilistic Robotics,   p110
           count_inv = 1.0/set_a->sample_count;
           r = drand48(   ) * count_inv;
           c = set_a->samples[0].weight;
           i = 0;
           m = 0;
           */
           while (  set_b->sample_count < pf->max_samples ) {
           sample_b = set_b->samples + set_b->sample_count++;
          
           if (  drand48(   ) < w_diff ) {
           sample_b->pose = (  pf->random_pose_fn )(  pf->random_pose_data );
           } else {
           // Can't (  easily ) combine low-variance sampler with KLD adaptive
           // sampling,   so we'll take the more traditional route.
           /*
           // Low-variance resampler,   taken from Probabilistic Robotics,   p110
           U = r + m * count_inv;
           while(  U>c )
           {
           i++;
           // Handle wrap-around by resetting counters and picking a new random
           // number
           if(  i >= set_a->sample_count )
           {
           r = drand48(   ) * count_inv;
           c = set_a->samples[0].weight;
           i = 0;
           m = 0;
           U = r + m * count_inv;
           continue;
           }
           c += set_a->samples[i].weight;
           }
           m++;
           */
          
           // Naive discrete event sampler
           double r;
           r = drand48(   );
           for (  i = 0; i < set_a->sample_count; i++ ) {
           if (  (  c[i] <= r ) && (  r < c[i + 1] ) ) {
           break;
           }
           }
           assert(  i < set_a->sample_count );
          
           sample_a = set_a->samples + i;
          
           assert(  sample_a->weight > 0 );
          
           // Add sample to list
           sample_b->pose = sample_a->pose;
           }
          
           sample_b->weight = 1.0;
           total += sample_b->weight;
          
           // Add sample to histogram
           pf_kdtree_insert(  set_b->kdtree,   sample_b->pose,   sample_b->weight );
          
           // See if we have enough samples yet
           if (  set_b->sample_count > pf_resample_limit(  pf,   set_b->kdtree->leaf_count ) ) {
           break;
           }
           }
          
           // Reset averages,   to avoid spiraling off into complete randomness.
           if (  w_diff > 0.0 ) {
           pf->w_slow = pf->w_fast = 0.0;
           }
          
           // fprintf(  stderr,   "\n\n" );
          
           // Normalize weights
           for (  i = 0; i < set_b->sample_count; i++ ) {
           sample_b = set_b->samples + i;
           sample_b->weight /= total;
           }
          
           // Re-compute cluster statistics
           pf_cluster_stats(  pf,   set_b );
          
           // Use the newly created sample set
           pf->current_set = (  pf->current_set + 1 ) % 2;
          
           pf_update_converged(  pf );
          
           free(  c );
          }
          
          
          // Compute the required number of samples,   given that there are k bins
          // with samples in them. This is taken directly from Fox et al.
     430  int pf_resample_limit(  pf_t * pf,   int k )
          {
           double a,   b,   c,   x;
           int n;
          
           if (  k <= 1 ) {
           return pf->max_samples;
           }
          
           a = 1;
           b = 2 / (  9 * (  (  double ) k - 1 ) );
           c = sqrt(  2 / (  9 * (  (  double ) k - 1 ) ) ) * pf->pop_z;
           x = a - b + c;
          
           n = (  int ) ceil(  (  k - 1 ) / (  2 * pf->pop_err ) * x * x * x );
          
           if (  n < pf->min_samples ) {
           return pf->min_samples;
           }
           if (  n > pf->max_samples ) {
           return pf->max_samples;
           }
          
           return n;
          }
          
          
          // Re-compute the cluster statistics for a sample set
     458  void pf_cluster_stats(  pf_t * pf,   pf_sample_set_t * set )
          {
           (  void )pf;
           int i,   j,   k,   cidx;
           pf_sample_t * sample;
           pf_cluster_t * cluster;
          
           // Workspace
           double m[4],   c[2][2];
           size_t count;
           double weight;
          
           // Cluster the samples
           pf_kdtree_cluster(  set->kdtree );
          
           // Initialize cluster stats
           set->cluster_count = 0;
          
           for (  i = 0; i < set->cluster_max_count; i++ ) {
           cluster = set->clusters + i;
           cluster->count = 0;
           cluster->weight = 0;
           cluster->mean = pf_vector_zero(   );
           cluster->cov = pf_matrix_zero(   );
          
           for (  j = 0; j < 4; j++ ) {
           cluster->m[j] = 0.0;
           }
           for (  j = 0; j < 2; j++ ) {
           for (  k = 0; k < 2; k++ ) {
           cluster->c[j][k] = 0.0;
           }
           }
           }
          
           // Initialize overall filter stats
           count = 0;
           weight = 0.0;
           set->mean = pf_vector_zero(   );
           set->cov = pf_matrix_zero(   );
           for (  j = 0; j < 4; j++ ) {
           m[j] = 0.0;
           }
           for (  j = 0; j < 2; j++ ) {
           for (  k = 0; k < 2; k++ ) {
           c[j][k] = 0.0;
           }
           }
          
           // Compute cluster stats
           for (  i = 0; i < set->sample_count; i++ ) {
           sample = set->samples + i;
          
           // printf(  "%d %f %f %f\n",   i,   sample->pose.v[0],   sample->pose.v[1],   sample->pose.v[2] );
          
           // Get the cluster label for this sample
           cidx = pf_kdtree_get_cluster(  set->kdtree,   sample->pose );
           assert(  cidx >= 0 );
           if (  cidx >= set->cluster_max_count ) {
           continue;
           }
           if (  cidx + 1 > set->cluster_count ) {
           set->cluster_count = cidx + 1;
           }
          
           cluster = set->clusters + cidx;
          
           cluster->count += 1;
           cluster->weight += sample->weight;
          
           count += 1;
           weight += sample->weight;
          
           // Compute mean
           cluster->m[0] += sample->weight * sample->pose.v[0];
           cluster->m[1] += sample->weight * sample->pose.v[1];
           cluster->m[2] += sample->weight * cos(  sample->pose.v[2] );
           cluster->m[3] += sample->weight * sin(  sample->pose.v[2] );
          
           m[0] += sample->weight * sample->pose.v[0];
           m[1] += sample->weight * sample->pose.v[1];
           m[2] += sample->weight * cos(  sample->pose.v[2] );
           m[3] += sample->weight * sin(  sample->pose.v[2] );
          
           // Compute covariance in linear components
           for (  j = 0; j < 2; j++ ) {
           for (  k = 0; k < 2; k++ ) {
           cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
           c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
           }
           }
           }
          
           // Normalize
           for (  i = 0; i < set->cluster_count; i++ ) {
           cluster = set->clusters + i;
          
           cluster->mean.v[0] = cluster->m[0] / cluster->weight;
           cluster->mean.v[1] = cluster->m[1] / cluster->weight;
           cluster->mean.v[2] = atan2(  cluster->m[3],   cluster->m[2] );
          
           cluster->cov = pf_matrix_zero(   );
          
           // Covariance in linear components
           for (  j = 0; j < 2; j++ ) {
           for (  k = 0; k < 2; k++ ) {
           cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
           cluster->mean.v[j] * cluster->mean.v[k];
           }
           }
          
           // Covariance in angular components; I think this is the correct
           // formula for circular statistics.
           cluster->cov.m[2][2] = -2 * log(  
           sqrt(  
           cluster->m[2] * cluster->m[2] +
           cluster->m[3] * cluster->m[3] ) );
          
           // printf(  "cluster %d %d %f (  %f %f %f )\n",   i,   cluster->count,   cluster->weight,  
           // cluster->mean.v[0],   cluster->mean.v[1],   cluster->mean.v[2] );
           // pf_matrix_fprintf(  cluster->cov,   stdout,   "%e" );
           }
          
           // Compute overall filter stats
           set->mean.v[0] = m[0] / weight;
           set->mean.v[1] = m[1] / weight;
           set->mean.v[2] = atan2(  m[3],   m[2] );
          
           // Covariance in linear components
           for (  j = 0; j < 2; j++ ) {
           for (  k = 0; k < 2; k++ ) {
           set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
           }
           }
          
           // Covariance in angular components; I think this is the correct
           // formula for circular statistics.
           set->cov.m[2][2] = -2 * log(  sqrt(  m[2] * m[2] + m[3] * m[3] ) );
          }
          
          
          // Compute the CEP statistics (  mean and variance ).
          // void pf_get_cep_stats(  pf_t * pf,   pf_vector_t * mean,   double * var )
          // {
          // int i;
          // double mn,   mx,   my,   mrr;
          // pf_sample_set_t * set;
          // pf_sample_t * sample;
          
          // set = pf->sets + pf->current_set;
          
          // mn = 0.0;
          // mx = 0.0;
          // my = 0.0;
          // mrr = 0.0;
          
          // for (  i = 0; i < set->sample_count; i++ ) {
          // sample = set->samples + i;
          
          // mn += sample->weight;
          // mx += sample->weight * sample->pose.v[0];
          // my += sample->weight * sample->pose.v[1];
          // mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
          // mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
          // }
          
          // mean->v[0] = mx / mn;
          // mean->v[1] = my / mn;
          // mean->v[2] = 0.0;
          
          // *var = mrr / mn - (  mx * mx / (  mn * mn ) + my * my / (  mn * mn ) );
          // }
          
          
          // Get the statistics for a particular cluster.
     633  int pf_get_cluster_stats(  
           pf_t * pf,   int clabel,   double * weight,  
           pf_vector_t * mean,   pf_matrix_t * cov )
          {
           pf_sample_set_t * set;
           pf_cluster_t * cluster;
          
           set = pf->sets + pf->current_set;
          
           if (  clabel >= set->cluster_count ) {
           return 0;
           }
           cluster = set->clusters + clabel;
          
           *weight = cluster->weight;
           *mean = cluster->mean;
           *cov = cluster->cov;
          
           return 1;
          }

navigation2/nav2_amcl/src/pf/pf_draw.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Particle filter; drawing routines
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
           *************************************************************************/
          
          #pragma GCC diagnostic ignored "-Wpedantic"
          #ifdef INCLUDE_RTKGUI
          
          #include <assert.h>
          #include <math.h>
          #include <stdlib.h>
          
          
          #include <rtk.h>
          
          #include "nav2_amcl/pf/pf.hpp"
          #include "nav2_amcl/pf/pf_pdf.hpp"
          #include "nav2_amcl/pf/pf_kdtree.hpp"
          
          // Draw the statistics
      43  void pf_draw_statistics(  pf_t * pf,   rtk_fig_t * fig );
          
          
          // Draw the sample set
      47  void pf_draw_samples(  pf_t * pf,   rtk_fig_t * fig,   int max_samples )
          {
           int i;
           double px,   py,   pa;
           pf_sample_set_t * set;
           pf_sample_t * sample;
          
           set = pf->sets + pf->current_set;
           max_samples = MIN(  max_samples,   set->sample_count );
          
           for (  i = 0; i < max_samples; i++ ) {
           sample = set->samples + i;
          
           px = sample->pose.v[0];
           py = sample->pose.v[1];
           pa = sample->pose.v[2];
          
           // printf(  "%f %f\n",   px,   py );
          
           rtk_fig_point(  fig,   px,   py );
           rtk_fig_arrow(  fig,   px,   py,   pa,   0.1,   0.02 );
           // rtk_fig_rectangle(  fig,   px,   py,   0,   0.1,   0.1,   0 );
           }
          }
          
          
          // Draw the hitogram (  kd tree )
      74  void pf_draw_hist(  pf_t * pf,   rtk_fig_t * fig )
          {
           pf_sample_set_t * set;
          
           set = pf->sets + pf->current_set;
          
           rtk_fig_color(  fig,   0.0,   0.0,   1.0 );
           pf_kdtree_draw(  set->kdtree,   fig );
          }
          
          
          // Draw the CEP statistics
          // void pf_draw_cep_stats(  pf_t * pf,   rtk_fig_t * fig )
          // {
          // pf_vector_t mean;
          // double var;
          
          // pf_get_cep_stats(  pf,   &mean,   &var );
          // var = sqrt(  var );
          
          // rtk_fig_color(  fig,   0,   0,   1 );
          // rtk_fig_ellipse(  fig,   mean.v[0],   mean.v[1],   mean.v[2],   3 * var,   3 * var,   0 );
          // }
          
          // Draw the cluster statistics
      99  void pf_draw_cluster_stats(  pf_t * pf,   rtk_fig_t * fig )
          {
           int i;
           pf_cluster_t * cluster;
           pf_sample_set_t * set;
           pf_vector_t mean;
           pf_matrix_t cov;
           pf_matrix_t r,   d;
           double weight,   o,   d1,   d2;
          
           set = pf->sets + pf->current_set;
          
           for (  i = 0; i < set->cluster_count; i++ ) {
           cluster = set->clusters + i;
          
           weight = cluster->weight;
           mean = cluster->mean;
           cov = cluster->cov;
          
           // Compute unitary representation S = R D R^T
           pf_matrix_unitary(  &r,   &d,   cov );
          
           /* Debugging
           printf(  "mean = \n" );
           pf_vector_fprintf(  mean,   stdout,   "%e" );
           printf(  "cov = \n" );
           pf_matrix_fprintf(  cov,   stdout,   "%e" );
           printf(  "r = \n" );
           pf_matrix_fprintf(  r,   stdout,   "%e" );
           printf(  "d = \n" );
           pf_matrix_fprintf(  d,   stdout,   "%e" );
           */
          
           // Compute the orientation of the error ellipse (  first eigenvector )
           o = atan2(  r.m[1][0],   r.m[0][0] );
           d1 = 6 * sqrt(  d.m[0][0] );
           d2 = 6 * sqrt(  d.m[1][1] );
          
           if (  d1 > 1e-3 && d2 > 1e-3 ) {
           // Draw the error ellipse
           rtk_fig_ellipse(  fig,   mean.v[0],   mean.v[1],   o,   d1,   d2,   0 );
           rtk_fig_line_ex(  fig,   mean.v[0],   mean.v[1],   o,   d1 );
           rtk_fig_line_ex(  fig,   mean.v[0],   mean.v[1],   o + M_PI / 2,   d2 );
           }
          
           // Draw a direction indicator
           rtk_fig_arrow(  fig,   mean.v[0],   mean.v[1],   mean.v[2],   0.50,   0.10 );
           rtk_fig_arrow(  fig,   mean.v[0],   mean.v[1],   mean.v[2] + 3 * sqrt(  cov.m[2][2] ),   0.50,   0.10 );
           rtk_fig_arrow(  fig,   mean.v[0],   mean.v[1],   mean.v[2] - 3 * sqrt(  cov.m[2][2] ),   0.50,   0.10 );
           }
          }
          #endif

navigation2/nav2_amcl/src/pf/pf_kdtree.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: kd-tree functions
           * Author: Andrew Howard
           * Date: 18 Dec 2002
           * CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
           *************************************************************************/
          
          #include <assert.h>
          #include <math.h>
          #include <stdlib.h>
          #include <string.h>
          
          
          #include "nav2_amcl/pf/pf_vector.hpp"
          #include "nav2_amcl/pf/pf_kdtree.hpp"
          
          
          // Compare keys to see if they are equal
      39  static int pf_kdtree_equal(  pf_kdtree_t * self,   int key_a[],   int key_b[] );
          
          // Insert a node into the tree
      42  static pf_kdtree_node_t * pf_kdtree_insert_node(  
           pf_kdtree_t * self,   pf_kdtree_node_t * parent,  
           pf_kdtree_node_t * node,   int key[],   double value );
          
          // Recursive node search
      47  static pf_kdtree_node_t * pf_kdtree_find_node(  
           pf_kdtree_t * self,   pf_kdtree_node_t * node,  
           int key[] );
          
          // Recursively label nodes in this cluster
      52  static void pf_kdtree_cluster_node(  pf_kdtree_t * self,   pf_kdtree_node_t * node,   int depth );
          
          // Recursive node printing
          // static void pf_kdtree_print_node(  pf_kdtree_t *self,   pf_kdtree_node_t *node );
          
          
          #ifdef INCLUDE_RTKGUI
          
          // Recursively draw nodes
      61  static void pf_kdtree_draw_node(  pf_kdtree_t * self,   pf_kdtree_node_t * node,   rtk_fig_t * fig );
          
          #endif
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Create a tree
      68  pf_kdtree_t * pf_kdtree_alloc(  int max_size )
          {
           pf_kdtree_t * self;
          
           self = calloc(  1,   sizeof(  pf_kdtree_t ) );
          
           self->size[0] = 0.50;
           self->size[1] = 0.50;
           self->size[2] = (  10 * M_PI / 180 );
          
           self->root = NULL;
          
           self->node_count = 0;
           self->node_max_count = max_size;
           self->nodes = calloc(  self->node_max_count,   sizeof(  pf_kdtree_node_t ) );
          
           self->leaf_count = 0;
          
           return self;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Destroy a tree
      92  void pf_kdtree_free(  pf_kdtree_t * self )
          {
           free(  self->nodes );
           free(  self );
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Clear all entries from the tree
     101  void pf_kdtree_clear(  pf_kdtree_t * self )
          {
           self->root = NULL;
           self->leaf_count = 0;
           self->node_count = 0;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Insert a pose into the tree.
     111  void pf_kdtree_insert(  pf_kdtree_t * self,   pf_vector_t pose,   double value )
          {
           int key[3];
          
           key[0] = floor(  pose.v[0] / self->size[0] );
           key[1] = floor(  pose.v[1] / self->size[1] );
           key[2] = floor(  pose.v[2] / self->size[2] );
          
           self->root = pf_kdtree_insert_node(  self,   NULL,   self->root,   key,   value );
          
           // Test code
           /*
           printf(  "find %d %d %d\n",   key[0],   key[1],   key[2] );
           assert(  pf_kdtree_find_node(  self,   self->root,   key ) != NULL );
          
           pf_kdtree_print_node(  self,   self->root );
          
           printf(  "\n" );
          
           for (  i = 0; i < self->node_count; i++ )
           {
           node = self->nodes + i;
           if (  node->leaf )
           {
           printf(  "find %d %d %d\n",   node->key[0],   node->key[1],   node->key[2] );
           assert(  pf_kdtree_find_node(  self,   self->root,   node->key ) == node );
           }
           }
           printf(  "\n\n" );
           */
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Determine the probability estimate for the given pose. TODO: this
          // should do a kernel density estimate rather than a simple histogram.
          // double pf_kdtree_get_prob(  pf_kdtree_t * self,   pf_vector_t pose )
          // {
          // int key[3];
          // pf_kdtree_node_t * node;
          
          // key[0] = floor(  pose.v[0] / self->size[0] );
          // key[1] = floor(  pose.v[1] / self->size[1] );
          // key[2] = floor(  pose.v[2] / self->size[2] );
          
          // node = pf_kdtree_find_node(  self,   self->root,   key );
          // if (  node == NULL ) {
          // return 0.0;
          // }
          // return node->value;
          // }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Determine the cluster label for the given pose
     166  int pf_kdtree_get_cluster(  pf_kdtree_t * self,   pf_vector_t pose )
          {
           int key[3];
           pf_kdtree_node_t * node;
          
           key[0] = floor(  pose.v[0] / self->size[0] );
           key[1] = floor(  pose.v[1] / self->size[1] );
           key[2] = floor(  pose.v[2] / self->size[2] );
          
           node = pf_kdtree_find_node(  self,   self->root,   key );
           if (  node == NULL ) {
           return -1;
           }
           return node->cluster;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Compare keys to see if they are equal
     185  int pf_kdtree_equal(  pf_kdtree_t * self,   int key_a[],   int key_b[] )
          {
           (  void )self;
           // double a,   b;
          
           if (  key_a[0] != key_b[0] ) {
           return 0;
           }
           if (  key_a[1] != key_b[1] ) {
           return 0;
           }
          
           if (  key_a[2] != key_b[2] ) {
           return 0;
           }
          
           /* TODO: make this work (  pivot selection needs fixing,   too )
           // Normalize angles
           a = key_a[2] * self->size[2];
           a = atan2(  sin(  a ),   cos(  a ) ) / self->size[2];
           b = key_b[2] * self->size[2];
           b = atan2(  sin(  b ),   cos(  b ) ) / self->size[2];
          
           if (  (  int ) a != (  int ) b )
           return 0;
           */
          
           return 1;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Insert a node into the tree
     218  pf_kdtree_node_t * pf_kdtree_insert_node(  
           pf_kdtree_t * self,   pf_kdtree_node_t * parent,  
           pf_kdtree_node_t * node,   int key[],   double value )
          {
           int i;
           int split,   max_split;
          
           // If the node doesnt exist yet...
           if (  node == NULL ) {
           assert(  self->node_count < self->node_max_count );
           node = self->nodes + self->node_count++;
           memset(  node,   0,   sizeof(  pf_kdtree_node_t ) );
          
           node->leaf = 1;
          
           if (  parent == NULL ) {
           node->depth = 0;
           } else {
           node->depth = parent->depth + 1;
           }
          
           for (  i = 0; i < 3; i++ ) {
           node->key[i] = key[i];
           }
          
           node->value = value;
           self->leaf_count += 1;
           } else if (  node->leaf ) { // If the node exists,   and it is a leaf node...
           // If the keys are equal,   increment the value
           if (  pf_kdtree_equal(  self,   key,   node->key ) ) {
           node->value += value;
           } else { // The keys are not equal,   so split this node
           // Find the dimension with the largest variance and do a mean
           // split
           max_split = 0;
           node->pivot_dim = -1;
           for (  i = 0; i < 3; i++ ) {
           split = abs(  key[i] - node->key[i] );
           if (  split > max_split ) {
           max_split = split;
           node->pivot_dim = i;
           }
           }
           assert(  node->pivot_dim >= 0 );
          
           node->pivot_value = (  key[node->pivot_dim] + node->key[node->pivot_dim] ) / 2.0;
          
           if (  key[node->pivot_dim] < node->pivot_value ) {
           node->children[0] = pf_kdtree_insert_node(  self,   node,   NULL,   key,   value );
           node->children[1] = pf_kdtree_insert_node(  self,   node,   NULL,   node->key,   node->value );
           } else {
           node->children[0] = pf_kdtree_insert_node(  self,   node,   NULL,   node->key,   node->value );
           node->children[1] = pf_kdtree_insert_node(  self,   node,   NULL,   key,   value );
           }
          
           node->leaf = 0;
           self->leaf_count -= 1;
           }
           } else { // If the node exists,   and it has children...
           assert(  node->children[0] != NULL );
           assert(  node->children[1] != NULL );
          
           if (  key[node->pivot_dim] < node->pivot_value ) {
           pf_kdtree_insert_node(  self,   node,   node->children[0],   key,   value );
           } else {
           pf_kdtree_insert_node(  self,   node,   node->children[1],   key,   value );
           }
           }
          
           return node;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Recursive node search
     293  pf_kdtree_node_t * pf_kdtree_find_node(  pf_kdtree_t * self,   pf_kdtree_node_t * node,   int key[] )
          {
           if (  node->leaf ) {
           // printf(  "find : leaf %p %d %d %d\n",   node,   node->key[0],   node->key[1],   node->key[2] );
          
           // If the keys are the same...
           if (  pf_kdtree_equal(  self,   key,   node->key ) ) {
           return node;
           } else {
           return NULL;
           }
           } else {
           // printf(  "find : brch %p %d %f\n",   node,   node->pivot_dim,   node->pivot_value );
          
           assert(  node->children[0] != NULL );
           assert(  node->children[1] != NULL );
          
           // If the keys are different...
           if (  key[node->pivot_dim] < node->pivot_value ) {
           return pf_kdtree_find_node(  self,   node->children[0],   key );
           } else {
           return pf_kdtree_find_node(  self,   node->children[1],   key );
           }
           }
          
           return NULL;
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Recursive node printing
          /*
          void pf_kdtree_print_node(  pf_kdtree_t *self,   pf_kdtree_node_t *node )
          {
           if (  node->leaf )
           {
           printf(  "(  %+02d %+02d %+02d )\n",   node->key[0],   node->key[1],   node->key[2] );
           printf(  "%*s",   node->depth * 11,   "" );
           }
           else
           {
           printf(  "(  %+02d %+02d %+02d ) ",   node->key[0],   node->key[1],   node->key[2] );
           pf_kdtree_print_node(  self,   node->children[0] );
           pf_kdtree_print_node(  self,   node->children[1] );
           }
           return;
          }
          */
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Cluster the leaves in the tree
     345  void pf_kdtree_cluster(  pf_kdtree_t * self )
          {
           int i;
           int queue_count,   cluster_count;
           pf_kdtree_node_t ** queue,   * node;
          
           queue_count = 0;
           queue = calloc(  self->node_count,   sizeof(  queue[0] ) );
          
           // Put all the leaves in a queue
           for (  i = 0; i < self->node_count; i++ ) {
           node = self->nodes + i;
           if (  node->leaf ) {
           node->cluster = -1;
           assert(  queue_count < self->node_count );
           queue[queue_count++] = node;
          
           // TESTING; remove
           assert(  node == pf_kdtree_find_node(  self,   self->root,   node->key ) );
           }
           }
          
           cluster_count = 0;
          
           // Do connected components for each node
           while (  queue_count > 0 ) {
           node = queue[--queue_count];
          
           // If this node has already been labelled,   skip it
           if (  node->cluster >= 0 ) {
           continue;
           }
          
           // Assign a label to this cluster
           node->cluster = cluster_count++;
          
           // Recursively label nodes in this cluster
           pf_kdtree_cluster_node(  self,   node,   0 );
           }
          
           free(  queue );
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Recursively label nodes in this cluster
     391  void pf_kdtree_cluster_node(  pf_kdtree_t * self,   pf_kdtree_node_t * node,   int depth )
          {
           int i;
           int nkey[3];
           pf_kdtree_node_t * nnode;
          
           for (  i = 0; i < 3 * 3 * 3; i++ ) {
           nkey[0] = node->key[0] + (  i / 9 ) - 1;
           nkey[1] = node->key[1] + (  (  i % 9 ) / 3 ) - 1;
           nkey[2] = node->key[2] + (  (  i % 9 ) % 3 ) - 1;
          
           nnode = pf_kdtree_find_node(  self,   self->root,   nkey );
           if (  nnode == NULL ) {
           continue;
           }
          
           assert(  nnode->leaf );
          
           // This node already has a label; skip it. The label should be
           // consistent,   however.
           if (  nnode->cluster >= 0 ) {
           assert(  nnode->cluster == node->cluster );
           continue;
           }
          
           // Label this node and recurse
           nnode->cluster = node->cluster;
          
           pf_kdtree_cluster_node(  self,   nnode,   depth + 1 );
           }
          }
          
          
          #ifdef INCLUDE_RTKGUI
          
          ////////////////////////////////////////////////////////////////////////////////
          // Draw the tree
     428  void pf_kdtree_draw(  pf_kdtree_t * self,   rtk_fig_t * fig )
          {
           if (  self->root != NULL ) {
           pf_kdtree_draw_node(  self,   self->root,   fig );
           }
          }
          
          
          ////////////////////////////////////////////////////////////////////////////////
          // Recursively draw nodes
     438  void pf_kdtree_draw_node(  pf_kdtree_t * self,   pf_kdtree_node_t * node,   rtk_fig_t * fig )
          {
           double ox,   oy;
           char text[64];
          
           if (  node->leaf ) {
           ox = (  node->key[0] + 0.5 ) * self->size[0];
           oy = (  node->key[1] + 0.5 ) * self->size[1];
          
           rtk_fig_rectangle(  fig,   ox,   oy,   0.0,   self->size[0],   self->size[1],   0 );
          
           // snprintf(  text,   sizeof(  text ),   "%0.3f",   node->value );
           // rtk_fig_text(  fig,   ox,   oy,   0.0,   text );
          
           snprintf(  text,   sizeof(  text ),   "%d",   node->cluster );
           rtk_fig_text(  fig,   ox,   oy,   0.0,   text );
           } else {
           assert(  node->children[0] != NULL );
           assert(  node->children[1] != NULL );
           pf_kdtree_draw_node(  self,   node->children[0],   fig );
           pf_kdtree_draw_node(  self,   node->children[1],   fig );
           }
          }
          
          #endif

navigation2/nav2_amcl/src/pf/pf_pdf.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Useful pdf functions
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
           *************************************************************************/
          
          #include <assert.h>
          #include <math.h>
          #include <stdlib.h>
          #include <string.h>
          // #include <gsl/gsl_rng.h>
          // #include <gsl/gsl_randist.h>
          
          #include "nav2_amcl/pf/pf_pdf.hpp"
          
          #include "nav2_amcl/portable_utils.hpp"
          
          // Random number generator seed value
          static unsigned int pf_pdf_seed;
          
          
          /**************************************************************************
           * Gaussian
           *************************************************************************/
          
          // Create a gaussian pdf
      48  pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(  pf_vector_t x,   pf_matrix_t cx )
          {
           pf_matrix_t cd;
           pf_pdf_gaussian_t * pdf;
          
           pdf = calloc(  1,   sizeof(  pf_pdf_gaussian_t ) );
          
           pdf->x = x;
           pdf->cx = cx;
           // pdf->cxi = pf_matrix_inverse(  cx,   &pdf->cxdet );
          
           // Decompose the convariance matrix into a rotation
           // matrix and a diagonal matrix.
           pf_matrix_unitary(  &pdf->cr,   &cd,   pdf->cx );
           pdf->cd.v[0] = sqrt(  cd.m[0][0] );
           pdf->cd.v[1] = sqrt(  cd.m[1][1] );
           pdf->cd.v[2] = sqrt(  cd.m[2][2] );
          
           // Initialize the random number generator
           // pdf->rng = gsl_rng_alloc(  gsl_rng_taus );
           // gsl_rng_set(  pdf->rng,   ++pf_pdf_seed );
           srand48(  ++pf_pdf_seed );
          
           return pdf;
          }
          
          
          // Destroy the pdf
      76  void pf_pdf_gaussian_free(  pf_pdf_gaussian_t * pdf )
          {
           // gsl_rng_free(  pdf->rng );
           free(  pdf );
          }
          
          
          /*
          // Compute the value of the pdf at some point [x].
          double pf_pdf_gaussian_value(  pf_pdf_gaussian_t *pdf,   pf_vector_t x )
          {
           int i,   j;
           pf_vector_t z;
           double zz,   p;
          
           z = pf_vector_sub(  x,   pdf->x );
          
           zz = 0;
           for (  i = 0; i < 3; i++ )
           for (  j = 0; j < 3; j++ )
           zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
          
           p = 1 / (  2 * M_PI * pdf->cxdet ) * exp(  -zz / 2 );
          
           return p;
          }
          */
          
          
          // Generate a sample from the pdf.
     106  pf_vector_t pf_pdf_gaussian_sample(  pf_pdf_gaussian_t * pdf )
          {
           int i,   j;
           pf_vector_t r;
           pf_vector_t x;
          
           // Generate a random vector
           for (  i = 0; i < 3; i++ ) {
           // r.v[i] = gsl_ran_gaussian(  pdf->rng,   pdf->cd.v[i] );
           r.v[i] = pf_ran_gaussian(  pdf->cd.v[i] );
           }
          
           for (  i = 0; i < 3; i++ ) {
           x.v[i] = pdf->x.v[i];
           for (  j = 0; j < 3; j++ ) {
           x.v[i] += pdf->cr.m[i][j] * r.v[j];
           }
           }
          
           return x;
          }
          
          // Draw randomly from a zero-mean Gaussian distribution,   with standard
          // deviation sigma.
          // We use the polar form of the Box-Muller transformation,   explained here:
          // http://www.taygeta.com/random/gaussian.html
     132  double pf_ran_gaussian(  double sigma )
          {
           double x1,   x2,   w,   r;
          
           do {
           do {
           r = drand48(   );
           } while (  r == 0.0 );
           x1 = 2.0 * r - 1.0;
           do {
           r = drand48(   );
           } while (  r == 0.0 );
           x2 = 2.0 * r - 1.0;
           w = x1 * x1 + x2 * x2;
           } while (  w > 1.0 || w == 0.0 );
          
           return sigma * x2 * sqrt(  -2.0 * log(  w ) / w );
          }

navigation2/nav2_amcl/src/pf/pf_vector.c

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          /**************************************************************************
           * Desc: Vector functions
           * Author: Andrew Howard
           * Date: 10 Dec 2002
           * CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
           *************************************************************************/
          
          #include <math.h>
          // #include <gsl/gsl_matrix.h>
          // #include <gsl/gsl_eigen.h>
          // #include <gsl/gsl_linalg.h>
          
          #include "nav2_amcl/pf/pf_vector.hpp"
          #include "nav2_amcl/pf/eig3.hpp"
          
          
          // Return a zero vector
      38  pf_vector_t pf_vector_zero(   )
          {
           pf_vector_t c;
          
           c.v[0] = 0.0;
           c.v[1] = 0.0;
           c.v[2] = 0.0;
          
           return c;
          }
          
          
          // // Check for NAN or INF in any component
          // int pf_vector_finite(  pf_vector_t a )
          // {
          // int i;
          
          // for (  i = 0; i < 3; i++ ) {
          // if (  !isfinite(  a.v[i] ) ) {
          // return 0;
          // }
          // }
          
          // return 1;
          // }
          
          
          // Print a vector
          // void pf_vector_fprintf(  pf_vector_t a,   FILE * file,   const char * fmt )
          // {
          // int i;
          
          // for (  i = 0; i < 3; i++ ) {
          // fprintf(  file,   fmt,   a.v[i] );
          // fprintf(  file,   " " );
          // }
          // fprintf(  file,   "\n" );
          // }
          
          
          // // Simple vector addition
          // pf_vector_t pf_vector_add(  pf_vector_t a,   pf_vector_t b )
          // {
          // pf_vector_t c;
          
          // c.v[0] = a.v[0] + b.v[0];
          // c.v[1] = a.v[1] + b.v[1];
          // c.v[2] = a.v[2] + b.v[2];
          
          // return c;
          // }
          
          
          // Simple vector subtraction
      92  pf_vector_t pf_vector_sub(  pf_vector_t a,   pf_vector_t b )
          {
           pf_vector_t c;
          
           c.v[0] = a.v[0] - b.v[0];
           c.v[1] = a.v[1] - b.v[1];
           c.v[2] = a.v[2] - b.v[2];
          
           return c;
          }
          
          
          // Transform from local to global coords (  a + b )
     105  pf_vector_t pf_vector_coord_add(  pf_vector_t a,   pf_vector_t b )
          {
           pf_vector_t c;
          
           c.v[0] = b.v[0] + a.v[0] * cos(  b.v[2] ) - a.v[1] * sin(  b.v[2] );
           c.v[1] = b.v[1] + a.v[0] * sin(  b.v[2] ) + a.v[1] * cos(  b.v[2] );
           c.v[2] = b.v[2] + a.v[2];
           c.v[2] = atan2(  sin(  c.v[2] ),   cos(  c.v[2] ) );
          
           return c;
          }
          
          
          // // Transform from global to local coords (  a - b )
          // pf_vector_t pf_vector_coord_sub(  pf_vector_t a,   pf_vector_t b )
          // {
          // pf_vector_t c;
          
          // c.v[0] = +(  a.v[0] - b.v[0] ) * cos(  b.v[2] ) + (  a.v[1] - b.v[1] ) * sin(  b.v[2] );
          // c.v[1] = -(  a.v[0] - b.v[0] ) * sin(  b.v[2] ) + (  a.v[1] - b.v[1] ) * cos(  b.v[2] );
          // c.v[2] = a.v[2] - b.v[2];
          // c.v[2] = atan2(  sin(  c.v[2] ),   cos(  c.v[2] ) );
          
          // return c;
          // }
          
          
          // Return a zero matrix
     133  pf_matrix_t pf_matrix_zero(   )
          {
           int i,   j;
           pf_matrix_t c;
          
           for (  i = 0; i < 3; i++ ) {
           for (  j = 0; j < 3; j++ ) {
           c.m[i][j] = 0.0;
           }
           }
          
           return c;
          }
          
          
          // // Check for NAN or INF in any component
          // int pf_matrix_finite(  pf_matrix_t a )
          // {
          // int i,   j;
          
          // for (  i = 0; i < 3; i++ ) {
          // for (  j = 0; j < 3; j++ ) {
          // if (  !isfinite(  a.m[i][j] ) ) {
          // return 0;
          // }
          // }
          // }
          
          // return 1;
          // }
          
          
          // Print a matrix
          // void pf_matrix_fprintf(  pf_matrix_t a,   FILE * file,   const char * fmt )
          // {
          // int i,   j;
          
          // for (  i = 0; i < 3; i++ ) {
          // for (  j = 0; j < 3; j++ ) {
          // fprintf(  file,   fmt,   a.m[i][j] );
          // fprintf(  file,   " " );
          // }
          // fprintf(  file,   "\n" );
          // }
          // }
          
          
          /*
          // Compute the matrix inverse
          pf_matrix_t pf_matrix_inverse(  pf_matrix_t a,   double *det )
          {
           double lndet;
           int signum;
           gsl_permutation *p;
           gsl_matrix_view A,   Ai;
          
           pf_matrix_t ai;
          
           A = gsl_matrix_view_array(  (  double* ) a.m,   3,   3 );
           Ai = gsl_matrix_view_array(  (  double* ) ai.m,   3,   3 );
          
           // Do LU decomposition
           p = gsl_permutation_alloc(  3 );
           gsl_linalg_LU_decomp(  &A.matrix,   p,   &signum );
          
           // Check for underflow
           lndet = gsl_linalg_LU_lndet(  &A.matrix );
           if (  lndet < -1000 )
           {
           //printf(  "underflow in matrix inverse lndet = %f",   lndet );
           gsl_matrix_set_zero(  &Ai.matrix );
           }
           else
           {
           // Compute inverse
           gsl_linalg_LU_invert(  &A.matrix,   p,   &Ai.matrix );
           }
          
           gsl_permutation_free(  p );
          
           if (  det )
           *det = exp(  lndet );
          
           return ai;
          }
          */
          
          
          // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
          // matrix [d] such that a = r d r^T.
     223  void pf_matrix_unitary(  pf_matrix_t * r,   pf_matrix_t * d,   pf_matrix_t a )
          {
           int i,   j;
           /*
           gsl_matrix *aa;
           gsl_vector *eval;
           gsl_matrix *evec;
           gsl_eigen_symmv_workspace *w;
          
           aa = gsl_matrix_alloc(  3,   3 );
           eval = gsl_vector_alloc(  3 );
           evec = gsl_matrix_alloc(  3,   3 );
           */
          
           double aa[3][3];
           double eval[3];
           double evec[3][3];
          
           for (  i = 0; i < 3; i++ ) {
           for (  j = 0; j < 3; j++ ) {
           // gsl_matrix_set(  aa,   i,   j,   a.m[i][j] );
           aa[i][j] = a.m[i][j];
           }
           }
          
           // Compute eigenvectors/values
           /*
           w = gsl_eigen_symmv_alloc(  3 );
           gsl_eigen_symmv(  aa,   eval,   evec,   w );
           gsl_eigen_symmv_free(  w );
           */
          
           eigen_decomposition(  aa,   evec,   eval );
          
           *d = pf_matrix_zero(   );
           for (  i = 0; i < 3; i++ ) {
           // d->m[i][i] = gsl_vector_get(  eval,   i );
           d->m[i][i] = eval[i];
           for (  j = 0; j < 3; j++ ) {
           // r->m[i][j] = gsl_matrix_get(  evec,   i,   j );
           r->m[i][j] = evec[i][j];
           }
           }
          
           // gsl_matrix_free(  evec );
           // gsl_vector_free(  eval );
           // gsl_matrix_free(  aa );
          }

navigation2/nav2_amcl/src/sensors/laser/beam_model.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include <math.h>
          #include <assert.h>
          
          #include "nav2_amcl/sensors/laser/laser.hpp"
          
          namespace nav2_amcl
          {
          
      30  BeamModel::BeamModel(  
           double z_hit,   double z_short,   double z_max,   double z_rand,   double sigma_hit,  
      32   double lambda_short,   double chi_outlier,   size_t max_beams,   map_t * map )
          : Laser(  max_beams,   map )
          {
           z_hit_ = z_hit;
           z_rand_ = z_rand;
           sigma_hit_ = sigma_hit;
           z_short_ = z_short;
           z_max_ = z_max;
           lambda_short_ = lambda_short;
           chi_outlier_ = chi_outlier;
          }
          
          // Determine the probability for the given pose
          double
      46  BeamModel::sensorFunction(  LaserData * data,   pf_sample_set_t * set )
          {
           BeamModel * self;
           int i,   j,   step;
           double z,   pz;
           double p;
           double map_range;
           double obs_range,   obs_bearing;
           double total_weight;
           pf_sample_t * sample;
           pf_vector_t pose;
          
           self = reinterpret_cast<BeamModel *>(  data->laser );
          
           total_weight = 0.0;
          
           // Compute the sample weights
           for (  j = 0; j < set->sample_count; j++ ) {
           sample = set->samples + j;
           pose = sample->pose;
          
           // Take account of the laser pose relative to the robot
           pose = pf_vector_coord_add(  self->laser_pose_,   pose );
          
           p = 1.0;
          
           step = (  data->range_count - 1 ) / (  self->max_beams_ - 1 );
           for (  i = 0; i < data->range_count; i += step ) {
           obs_range = data->ranges[i][0];
           obs_bearing = data->ranges[i][1];
          
           // Compute the range according to the map
           map_range = map_calc_range(  
           self->map_,   pose.v[0],   pose.v[1],  
           pose.v[2] + obs_bearing,   data->range_max );
           pz = 0.0;
          
           // Part 1: good,   but noisy,   hit
           z = obs_range - map_range;
           pz += self->z_hit_ * exp(  -(  z * z ) / (  2 * self->sigma_hit_ * self->sigma_hit_ ) );
          
           // Part 2: short reading from unexpected obstacle (  e.g.,   a person )
           if (  z < 0 ) {
           pz += self->z_short_ * self->lambda_short_ * exp(  -self->lambda_short_ * obs_range );
           }
          
           // Part 3: Failure to detect obstacle,   reported as max-range
           if (  obs_range == data->range_max ) {
           pz += self->z_max_ * 1.0;
           }
          
           // Part 4: Random measurements
           if (  obs_range < data->range_max ) {
           pz += self->z_rand_ * 1.0 / data->range_max;
           }
          
           // TODO(  ? ): outlier rejection for short readings
          
           assert(  pz <= 1.0 );
           assert(  pz >= 0.0 );
           // p *= pz;
           // here we have an ad-hoc weighting scheme for combining beam probs
           // works well,   though...
           p += pz * pz * pz;
           }
          
           sample->weight *= p;
           total_weight += sample->weight;
           }
          
           return total_weight;
          }
          
          bool
     120  BeamModel::sensorUpdate(  pf_t * pf,   LaserData * data )
          {
           if (  max_beams_ < 2 ) {
           return false;
           }
           pf_update_sensor(  pf,   (  pf_sensor_model_fn_t ) sensorFunction,   data );
          
           return true;
          }
          
          } // namespace nav2_amcl

navigation2/nav2_amcl/src/sensors/laser/laser.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include <sys/types.h>
          #include <math.h>
          #include <stdlib.h>
          #include <assert.h>
          
          #include "nav2_amcl/sensors/laser/laser.hpp"
          
          namespace nav2_amcl
          {
          
      32  Laser::Laser(  size_t max_beams,   map_t * map )
          : max_samples_(  0 ),   max_obs_(  0 ),   temp_obs_(  NULL )
          {
           max_beams_ = max_beams;
           map_ = map;
          }
          
      39  Laser::~Laser(   )
          {
           if (  temp_obs_ ) {
           for (  int k = 0; k < max_samples_; k++ ) {
           delete[] temp_obs_[k];
           }
           delete[] temp_obs_;
           }
          }
          
          void
      50  Laser::reallocTempData(  int new_max_samples,   int new_max_obs )
          {
           if (  temp_obs_ ) {
           for (  int k = 0; k < max_samples_; k++ ) {
           delete[] temp_obs_[k];
           }
           delete[] temp_obs_;
           }
           max_obs_ = new_max_obs;
           max_samples_ = fmax(  max_samples_,   new_max_samples );
          
           temp_obs_ = new double *[max_samples_](   );
           for (  int k = 0; k < max_samples_; k++ ) {
           temp_obs_[k] = new double[max_obs_](   );
           }
          }
          
          void
      68  Laser::SetLaserPose(  pf_vector_t & laser_pose )
          {
           laser_pose_ = laser_pose;
          }
          
          } // namespace nav2_amcl

navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          #include <math.h>
          #include <assert.h>
          
          #include "nav2_amcl/sensors/laser/laser.hpp"
          
          namespace nav2_amcl
          {
          
      30  LikelihoodFieldModel::LikelihoodFieldModel(  
           double z_hit,   double z_rand,   double sigma_hit,  
      32   double max_occ_dist,   size_t max_beams,   map_t * map )
          : Laser(  max_beams,   map )
          {
           z_hit_ = z_hit;
           z_rand_ = z_rand;
           sigma_hit_ = sigma_hit;
           map_update_cspace(  map,   max_occ_dist );
          }
          
          double
      42  LikelihoodFieldModel::sensorFunction(  LaserData * data,   pf_sample_set_t * set )
          {
           LikelihoodFieldModel * self;
           int i,   j,   step;
           double z,   pz;
           double p;
           double obs_range,   obs_bearing;
           double total_weight;
           pf_sample_t * sample;
           pf_vector_t pose;
           pf_vector_t hit;
          
           self = reinterpret_cast<LikelihoodFieldModel *>(  data->laser );
          
           total_weight = 0.0;
          
           // Compute the sample weights
           for (  j = 0; j < set->sample_count; j++ ) {
           sample = set->samples + j;
           pose = sample->pose;
          
           // Take account of the laser pose relative to the robot
           pose = pf_vector_coord_add(  self->laser_pose_,   pose );
          
           p = 1.0;
          
           // Pre-compute a couple of things
           double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
           double z_rand_mult = 1.0 / data->range_max;
          
           step = (  data->range_count - 1 ) / (  self->max_beams_ - 1 );
          
           // Step size must be at least 1
           if (  step < 1 ) {
           step = 1;
           }
          
           for (  i = 0; i < data->range_count; i += step ) {
           obs_range = data->ranges[i][0];
           obs_bearing = data->ranges[i][1];
          
           // This model ignores max range readings
           if (  obs_range >= data->range_max ) {
           continue;
           }
          
           // Check for NaN
           if (  obs_range != obs_range ) {
           continue;
           }
          
           pz = 0.0;
          
           // Compute the endpoint of the beam
           hit.v[0] = pose.v[0] + obs_range * cos(  pose.v[2] + obs_bearing );
           hit.v[1] = pose.v[1] + obs_range * sin(  pose.v[2] + obs_bearing );
          
           // Convert to map grid coords.
           int mi,   mj;
           mi = MAP_GXWX(  self->map_,   hit.v[0] );
           mj = MAP_GYWY(  self->map_,   hit.v[1] );
          
           // Part 1: Get distance from the hit to closest obstacle.
           // Off-map penalized as max distance
           if (  !MAP_VALID(  self->map_,   mi,   mj ) ) {
           z = self->map_->max_occ_dist;
           } else {
           z = self->map_->cells[MAP_INDEX(  self->map_,   mi,   mj )].occ_dist;
           }
           // Gaussian model
           // NOTE: this should have a normalization of 1/(  sqrt(  2pi )*sigma )
           pz += self->z_hit_ * exp(  -(  z * z ) / z_hit_denom );
           // Part 2: random measurements
           pz += self->z_rand_ * z_rand_mult;
          
           // TODO(  ? ): outlier rejection for short readings
          
           assert(  pz <= 1.0 );
           assert(  pz >= 0.0 );
           // p *= pz;
           // here we have an ad-hoc weighting scheme for combining beam probs
           // works well,   though...
           p += pz * pz * pz;
           }
          
           sample->weight *= p;
           total_weight += sample->weight;
           }
          
           return total_weight;
          }
          
          
          bool
     136  LikelihoodFieldModel::sensorUpdate(  pf_t * pf,   LaserData * data )
          {
           if (  max_beams_ < 2 ) {
           return false;
           }
           pf_update_sensor(  pf,   (  pf_sensor_model_fn_t ) sensorFunction,   data );
          
           return true;
          }
          
          } // namespace nav2_amcl

navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp

       1  /*
           * Player - One Hell of a Robot Server
           * Copyright (  C ) 2000 Brian Gerkey & Kasper Stoy
           * gerkey@usc.edu kaspers@robotics.usc.edu
           *
           * This library is free software; you can redistribute it and/or
           * modify it under the terms of the GNU Lesser General Public
           * License as published by the Free Software Foundation; either
           * version 2.1 of the License,   or (  at your option ) any later version.
           *
           * This library is distributed in the hope that it will be useful,  
           * but WITHOUT ANY WARRANTY; without even the implied warranty of
           * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
           * Lesser General Public License for more details.
           *
           * You should have received a copy of the GNU Lesser General Public
           * License along with this library; if not,   write to the Free Software
           * Foundation,   Inc.,   59 Temple Place,   Suite 330,   Boston,   MA 02111-1307 USA
           *
           */
          
          
          #include <math.h>
          #include <assert.h>
          
          #include "nav2_amcl/sensors/laser/laser.hpp"
          
          namespace nav2_amcl
          {
          
      31  LikelihoodFieldModelProb::LikelihoodFieldModelProb(  
           double z_hit,   double z_rand,   double sigma_hit,  
      33   double max_occ_dist,   bool do_beamskip,  
           double beam_skip_distance,  
           double beam_skip_threshold,  
           double beam_skip_error_threshold,  
      37   size_t max_beams,   map_t * map )
          : Laser(  max_beams,   map )
          {
           z_hit_ = z_hit;
           z_rand_ = z_rand;
           sigma_hit_ = sigma_hit;
           do_beamskip_ = do_beamskip;
           beam_skip_distance_ = beam_skip_distance;
           beam_skip_threshold_ = beam_skip_threshold;
           beam_skip_error_threshold_ = beam_skip_error_threshold;
           map_update_cspace(  map,   max_occ_dist );
          }
          
          // Determine the probability for the given pose
          double
      52  LikelihoodFieldModelProb::sensorFunction(  LaserData * data,   pf_sample_set_t * set )
          {
           LikelihoodFieldModelProb * self;
           int i,   j,   step;
           double z,   pz;
           double log_p;
           double obs_range,   obs_bearing;
           double total_weight;
           pf_sample_t * sample;
           pf_vector_t pose;
           pf_vector_t hit;
          
           self = reinterpret_cast<LikelihoodFieldModelProb *>(  data->laser );
          
           total_weight = 0.0;
          
           step = ceil(  (  data->range_count ) / static_cast<double>(  self->max_beams_ ) );
          
           // Step size must be at least 1
           if (  step < 1 ) {
           step = 1;
           }
          
           // Pre-compute a couple of things
           double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
           double z_rand_mult = 1.0 / data->range_max;
          
           double max_dist_prob = exp(  -(  self->map_->max_occ_dist * self->map_->max_occ_dist ) / z_hit_denom );
          
           // Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
           // prevents correct particles from getting down weighted because of unexpected obstacles
           // such as humans
          
           bool do_beamskip = self->do_beamskip_;
           double beam_skip_distance = self->beam_skip_distance_;
           double beam_skip_threshold = self->beam_skip_threshold_;
          
           // we only do beam skipping if the filter has converged
           if (  do_beamskip && !set->converged ) {
           do_beamskip = false;
           }
          
           // we need a count the no of particles for which the beam agreed with the map
           int * obs_count = new int[self->max_beams_](   );
          
           // we also need a mask of which observations to integrate (  to decide which beams to integrate to
           // all particles )
           bool * obs_mask = new bool[self->max_beams_](   );
          
           int beam_ind = 0;
          
           // realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
           bool realloc = false;
          
           if (  do_beamskip ) {
           if (  self->max_obs_ < self->max_beams_ ) {
           realloc = true;
           }
          
           if (  self->max_samples_ < set->sample_count ) {
           realloc = true;
           }
          
           if (  realloc ) {
           self->reallocTempData(  set->sample_count,   self->max_beams_ );
           fprintf(  stderr,   "Reallocing temp weights %d - %d\n",   self->max_samples_,   self->max_obs_ );
           }
           }
          
           // Compute the sample weights
           for (  j = 0; j < set->sample_count; j++ ) {
           sample = set->samples + j;
           pose = sample->pose;
          
           // Take account of the laser pose relative to the robot
           pose = pf_vector_coord_add(  self->laser_pose_,   pose );
          
           log_p = 0;
          
           beam_ind = 0;
          
           for (  i = 0; i < data->range_count; i += step,   beam_ind++ ) {
           obs_range = data->ranges[i][0];
           obs_bearing = data->ranges[i][1];
          
           // This model ignores max range readings
           if (  obs_range >= data->range_max ) {
           continue;
           }
          
           // Check for NaN
           if (  obs_range != obs_range ) {
           continue;
           }
          
           pz = 0.0;
          
           // Compute the endpoint of the beam
           hit.v[0] = pose.v[0] + obs_range * cos(  pose.v[2] + obs_bearing );
           hit.v[1] = pose.v[1] + obs_range * sin(  pose.v[2] + obs_bearing );
          
           // Convert to map grid coords.
           int mi,   mj;
           mi = MAP_GXWX(  self->map_,   hit.v[0] );
           mj = MAP_GYWY(  self->map_,   hit.v[1] );
          
           // Part 1: Get distance from the hit to closest obstacle.
           // Off-map penalized as max distance
          
           if (  !MAP_VALID(  self->map_,   mi,   mj ) ) {
           pz += self->z_hit_ * max_dist_prob;
           } else {
           z = self->map_->cells[MAP_INDEX(  self->map_,   mi,   mj )].occ_dist;
           if (  z < beam_skip_distance ) {
           obs_count[beam_ind] += 1;
           }
           pz += self->z_hit_ * exp(  -(  z * z ) / z_hit_denom );
           }
          
           // Gaussian model
           // NOTE: this should have a normalization of 1/(  sqrt(  2pi )*sigma )
          
           // Part 2: random measurements
           pz += self->z_rand_ * z_rand_mult;
          
           assert(  pz <= 1.0 );
           assert(  pz >= 0.0 );
          
           // TODO(  ? ): outlier rejection for short readings
          
           if (  !do_beamskip ) {
           log_p += log(  pz );
           } else {
           self->temp_obs_[j][beam_ind] = pz;
           }
           }
           if (  !do_beamskip ) {
           sample->weight *= exp(  log_p );
           total_weight += sample->weight;
           }
           }
          
           if (  do_beamskip ) {
           int skipped_beam_count = 0;
           for (  beam_ind = 0; beam_ind < self->max_beams_; beam_ind++ ) {
           if (  (  obs_count[beam_ind] / static_cast<double>(  set->sample_count ) ) > beam_skip_threshold ) {
           obs_mask[beam_ind] = true;
           } else {
           obs_mask[beam_ind] = false;
           skipped_beam_count++;
           }
           }
          
           // we check if there is at least a critical number of beams that agreed with the map
           // otherwise it probably indicates that the filter converged to a wrong solution
           // if that's the case we integrate all the beams and hope the filter might converge to
           // the right solution
           bool error = false;
          
           if (  skipped_beam_count >= (  beam_ind * self->beam_skip_error_threshold_ ) ) {
           fprintf(  
           stderr,  
           "Over %f%% of the observations were not in the map - pf may have converged to wrong pose -"
           " integrating all observations\n",  
           (  100 * self->beam_skip_error_threshold_ ) );
           error = true;
           }
          
           for (  j = 0; j < set->sample_count; j++ ) {
           sample = set->samples + j;
           pose = sample->pose;
          
           log_p = 0;
          
           for (  beam_ind = 0; beam_ind < self->max_beams_; beam_ind++ ) {
           if (  error || obs_mask[beam_ind] ) {
           log_p += log(  self->temp_obs_[j][beam_ind] );
           }
           }
          
           sample->weight *= exp(  log_p );
          
           total_weight += sample->weight;
           }
           }
          
           delete[] obs_count;
           delete[] obs_mask;
           return total_weight;
          }
          
          bool
     244  LikelihoodFieldModelProb::sensorUpdate(  pf_t * pf,   LaserData * data )
          {
           if (  max_beams_ < 2 ) {
           return false;
           }
           pf_update_sensor(  pf,   (  pf_sensor_model_fn_t ) sensorFunction,   data );
          
           return true;
          }
          
          } // namespace nav2_amcl

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Florian Gramss
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
          #define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "behaviortree_cpp_v3/behavior_tree.h"
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "behaviortree_cpp_v3/xml_parsing.h"
          #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
          
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @enum nav2_behavior_tree::BtStatus
           * @brief An enum class representing BT execution status
           */
      36  enum class BtStatus { SUCCEEDED,   FAILED,   CANCELED };
          
          /**
           * @class nav2_behavior_tree::BehaviorTreeEngine
           * @brief A class to create and handle behavior trees
           */
      42  class BehaviorTreeEngine
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
           * @param plugin_libraries vector of BT plugin library names to load
           */
      49   explicit BehaviorTreeEngine(  const std::vector<std::string> & plugin_libraries );
      50   virtual ~BehaviorTreeEngine(   ) {}
          
           /**
           * @brief Function to execute a BT at a specific rate
           * @param tree BT to execute
           * @param onLoop Function to execute on each iteration of BT execution
           * @param cancelRequested Function to check if cancel was requested during BT execution
           * @param loopTimeout Time period for each iteration of BT execution
           * @return nav2_behavior_tree::BtStatus Status of BT execution
           */
      60   BtStatus run(  
           BT::Tree * tree,  
           std::function<void(   )> onLoop,  
           std::function<bool(   )> cancelRequested,  
           std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(  10 ) );
          
           /**
           * @brief Function to create a BT from a XML string
           * @param xml_string XML string representing BT
           * @param blackboard Blackboard for BT
           * @return BT::Tree Created behavior tree
           */
      72   BT::Tree createTreeFromText(  
      73   const std::string & xml_string,  
      74   BT::Blackboard::Ptr blackboard );
          
           /**
           * @brief Function to create a BT from an XML file
           * @param file_path Path to BT XML file
           * @param blackboard Blackboard for BT
           * @return BT::Tree Created behavior tree
           */
      82   BT::Tree createTreeFromFile(  
      83   const std::string & file_path,  
      84   BT::Blackboard::Ptr blackboard );
          
           /**
           * @brief Function to explicitly reset all BT nodes to initial state
           * @param root_node Pointer to BT root node
           */
      90   void haltAllActions(  BT::TreeNode * root_node );
          
          protected:
           // The factory that will be used to dynamically construct the behavior tree
      94   BT::BehaviorTreeFactory factory_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "behaviortree_cpp_v3/action_node.h"
          #include "nav2_util/node_utils.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_behavior_tree/bt_conversions.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief Abstract class representing an action based BT node
           * @tparam ActionT Type of action
           */
          template<class ActionT>
      34  class BtActionNode : public BT::ActionNodeBase
          {
          public:
           /**
           * @brief A nav2_behavior_tree::BtActionNode constructor
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      43   BtActionNode(  
      44   const std::string & xml_tag_name,  
      45   const std::string & action_name,  
      46   const BT::NodeConfiguration & conf )
           : BT::ActionNodeBase(  xml_tag_name,   conf ),   action_name_(  action_name )
           {
           node_ = config(   ).blackboard->template get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           // Get the required items from the blackboard
           bt_loop_duration_ =
           config(   ).blackboard->template get<std::chrono::milliseconds>(  "bt_loop_duration" );
           server_timeout_ =
           config(   ).blackboard->template get<std::chrono::milliseconds>(  "server_timeout" );
           getInput<std::chrono::milliseconds>(  "server_timeout",   server_timeout_ );
          
           // Initialize the input and output messages
           goal_ = typename ActionT::Goal(   );
           result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult(   );
          
           std::string remapped_action_name;
           if (  getInput(  "server_name",   remapped_action_name ) ) {
           action_name_ = remapped_action_name;
           }
           createActionClient(  action_name_ );
          
           // Give the derive class a chance to do any initialization
           RCLCPP_DEBUG(  node_->get_logger(   ),   "\"%s\" BtActionNode initialized",   xml_tag_name.c_str(   ) );
           }
          
           BtActionNode(   ) = delete;
          
           virtual ~BtActionNode(   )
           {
           }
          
           /**
           * @brief Create instance of an action client
           * @param action_name Action name to create client for
           */
           void createActionClient(  const std::string & action_name )
           {
           // Now that we have the ROS node to use,   create the action client for this BT action
           action_client_ = rclcpp_action::create_client<ActionT>(  node_,   action_name,   callback_group_ );
          
           // Make sure the server is actually there before continuing
      92   RCLCPP_DEBUG(  node_->get_logger(   ),   "Waiting for \"%s\" action server",   action_name.c_str(   ) );
           static constexpr std::chrono::milliseconds wait_for_server_timeout =
           std::chrono::milliseconds(  1000 );
      95   if (  !action_client_->wait_for_action_server(  wait_for_server_timeout ) ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),   "\"%s\" action server not available after waiting for %li ms",  
           action_name.c_str(   ),   wait_for_server_timeout.count(   ) );
           throw std::runtime_error(  "Action server not available" );
           }
           }
          
           /**
           * @brief Any subclass of BtActionNode that accepts parameters must provide a
           * providedPorts method and call providedBasicPorts in it.
           * @param addition Additional ports to add to BT port list
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedBasicPorts(  BT::PortsList addition )
           {
           BT::PortsList basic = {
           BT::InputPort<std::string>(  "server_name",   "Action server name" ),  
           BT::InputPort<std::chrono::milliseconds>(  "server_timeout" )
           };
           basic.insert(  addition.begin(   ),   addition.end(   ) );
          
           return basic;
           }
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  {} );
           }
          
           // Derived classes can override any of the following methods to hook into the
           // processing for the action: on_tick,   on_wait_for_result,   and on_success
          
           /**
           * @brief Function to perform some user-defined operation on tick
           * Could do dynamic checks,   such as getting updates to values on the blackboard
           */
           virtual void on_tick(   )
           {
           }
          
           /**
           * @brief Function to perform some user-defined operation after a timeout
           * waiting for a result that hasn't been received yet. Also provides access to
           * the latest feedback message from the action server. Feedback will be nullptr
           * in subsequent calls to this function if no new feedback is received while waiting for a result.
           * @param feedback shared_ptr to latest feedback message,   nullptr if no new feedback was received
           */
           virtual void on_wait_for_result(  std::shared_ptr<const typename ActionT::Feedback>/*feedback*/ )
           {
           }
          
           /**
           * @brief Function to perform some user-defined operation upon successful
           * completion of the action. Could put a value on the blackboard.
           * @return BT::NodeStatus Returns SUCCESS by default,   user may override return another value
           */
           virtual BT::NodeStatus on_success(   )
           {
           return BT::NodeStatus::SUCCESS;
           }
          
           /**
           * @brief Function to perform some user-defined operation whe the action is aborted.
           * @return BT::NodeStatus Returns FAILURE by default,   user may override return another value
           */
           virtual BT::NodeStatus on_aborted(   )
           {
           return BT::NodeStatus::FAILURE;
           }
          
           /**
           * @brief Function to perform some user-defined operation when the action is cancelled.
           * @return BT::NodeStatus Returns SUCCESS by default,   user may override return another value
           */
           virtual BT::NodeStatus on_cancelled(   )
           {
           return BT::NodeStatus::SUCCESS;
           }
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override
           {
           // first step to be done only at the beginning of the Action
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           // setting the status to RUNNING to notify the BT Loggers (  if any )
           setStatus(  BT::NodeStatus::RUNNING );
          
           // user defined callback
           on_tick(   );
          
           send_new_goal(   );
           }
          
           try {
           // if new goal was sent and action server has not yet responded
           // check the future goal handle
           if (  future_goal_handle_ ) {
           auto elapsed = (  node_->now(   ) - time_goal_sent_ ).to_chrono<std::chrono::milliseconds>(   );
           if (  !is_future_goal_handle_complete(  elapsed ) ) {
           // return RUNNING if there is still some time before timeout happens
           if (  elapsed < server_timeout_ ) {
           return BT::NodeStatus::RUNNING;
           }
           // if server has taken more time than the specified timeout value return FAILURE
           RCLCPP_WARN(  
           node_->get_logger(   ),  
           "Timed out while waiting for action server to acknowledge goal request for %s",  
           action_name_.c_str(   ) );
           future_goal_handle_.reset(   );
           return BT::NodeStatus::FAILURE;
           }
           }
          
           // The following code corresponds to the "RUNNING" loop
           if (  rclcpp::ok(   ) && !goal_result_available_ ) {
           // user defined callback. May modify the value of "goal_updated_"
           on_wait_for_result(  feedback_ );
          
           // reset feedback to avoid stale information
           feedback_.reset(   );
          
           auto goal_status = goal_handle_->get_status(   );
           if (  goal_updated_ && (  goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
           goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ) )
           {
           goal_updated_ = false;
           send_new_goal(   );
           auto elapsed = (  node_->now(   ) - time_goal_sent_ ).to_chrono<std::chrono::milliseconds>(   );
           if (  !is_future_goal_handle_complete(  elapsed ) ) {
           if (  elapsed < server_timeout_ ) {
           return BT::NodeStatus::RUNNING;
           }
           RCLCPP_WARN(  
           node_->get_logger(   ),  
           "Timed out while waiting for action server to acknowledge goal request for %s",  
           action_name_.c_str(   ) );
           future_goal_handle_.reset(   );
           return BT::NodeStatus::FAILURE;
           }
           }
          
           callback_group_executor_.spin_some(   );
          
           // check if,   after invoking spin_some(   ),   we finally received the result
           if (  !goal_result_available_ ) {
           // Yield this Action,   returning RUNNING
           return BT::NodeStatus::RUNNING;
           }
           }
           } catch (  const std::runtime_error & e ) {
           if (  e.what(   ) == std::string(  "send_goal failed" ) ||
           e.what(   ) == std::string(  "Goal was rejected by the action server" ) )
           {
           // Action related failure that should not fail the tree,   but the node
           return BT::NodeStatus::FAILURE;
           } else {
           // Internal exception to propogate to the tree
           throw e;
           }
           }
          
           BT::NodeStatus status;
           switch (  result_.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED:
           status = on_success(   );
           break;
          
           case rclcpp_action::ResultCode::ABORTED:
           status = on_aborted(   );
           break;
          
           case rclcpp_action::ResultCode::CANCELED:
           status = on_cancelled(   );
           break;
          
           default:
           throw std::logic_error(  "BtActionNode::Tick: invalid status value" );
           }
          
           goal_handle_.reset(   );
           return status;
           }
          
           /**
           * @brief The other (  optional ) override required by a BT action. In this case,   we
           * make sure to cancel the ROS2 action if it is still running.
           */
           void halt(   ) override
           {
           if (  should_cancel_goal(   ) ) {
           auto future_cancel = action_client_->async_cancel_goal(  goal_handle_ );
           if (  callback_group_executor_.spin_until_future_complete(  future_cancel,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Failed to cancel action server for %s",   action_name_.c_str(   ) );
           }
           }
          
           setStatus(  BT::NodeStatus::IDLE );
           }
          
          protected:
           /**
           * @brief Function to check if current goal should be cancelled
           * @return bool True if current goal should be cancelled,   false otherwise
           */
           bool should_cancel_goal(   )
           {
           // Shut the node down if it is currently running
           if (  status(   ) != BT::NodeStatus::RUNNING ) {
           return false;
           }
          
           // No need to cancel the goal if goal handle is invalid
           if (  !goal_handle_ ) {
           return false;
           }
          
           callback_group_executor_.spin_some(   );
           auto status = goal_handle_->get_status(   );
          
           // Check if the goal is still executing
           return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
           }
          
           /**
           * @brief Function to send new goal to action server
           */
           void send_new_goal(   )
           {
           goal_result_available_ = false;
           auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions(   );
           send_goal_options.result_callback =
           [this](  const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result ) {
           if (  future_goal_handle_ ) {
           RCLCPP_DEBUG(  
           node_->get_logger(   ),  
           "Goal result for %s available,   but it hasn't received the goal response yet. "
           "It's probably a goal result for the last goal request",   action_name_.c_str(   ) );
           return;
           }
          
           // TODO(  #1652 ): a work around until rcl_action interface is updated
           // if goal ids are not matched,   the older goal call this callback so ignore the result
           // if matched,   it must be processed (  including aborted )
           if (  this->goal_handle_->get_goal_id(   ) == result.goal_id ) {
           goal_result_available_ = true;
           result_ = result;
           }
           };
           send_goal_options.feedback_callback =
           [this](  typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,  
           const std::shared_ptr<const typename ActionT::Feedback> feedback ) {
           feedback_ = feedback;
           };
          
           future_goal_handle_ = std::make_shared<
           std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(  
           action_client_->async_send_goal(  goal_,   send_goal_options ) );
           time_goal_sent_ = node_->now(   );
           }
          
           /**
           * @brief Function to check if the action server acknowledged a new goal
           * @param elapsed Duration since the last goal was sent and future goal handle has not completed.
           * After waiting for the future to complete,   this value is incremented with the timeout value.
           * @return boolean True if future_goal_handle_ returns SUCCESS,   False otherwise
           */
           bool is_future_goal_handle_complete(  std::chrono::milliseconds & elapsed )
           {
           auto remaining = server_timeout_ - elapsed;
          
           // server has already timed out,   no need to sleep
           if (  remaining <= std::chrono::milliseconds(  0 ) ) {
           future_goal_handle_.reset(   );
           return false;
           }
          
           auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
           auto result =
           callback_group_executor_.spin_until_future_complete(  *future_goal_handle_,   timeout );
           elapsed += timeout;
          
           if (  result == rclcpp::FutureReturnCode::INTERRUPTED ) {
           future_goal_handle_.reset(   );
           throw std::runtime_error(  "send_goal failed" );
           }
          
           if (  result == rclcpp::FutureReturnCode::SUCCESS ) {
           goal_handle_ = future_goal_handle_->get(   );
           future_goal_handle_.reset(   );
           if (  !goal_handle_ ) {
           throw std::runtime_error(  "Goal was rejected by the action server" );
           }
           return true;
           }
          
           return false;
           }
          
           /**
           * @brief Function to increment recovery count on blackboard if this node wraps a recovery
           */
           void increment_recovery_count(   )
           {
           int recovery_count = 0;
           config(   ).blackboard->template get<int>(  "number_recoveries",   recovery_count ); // NOLINT
           recovery_count += 1;
           config(   ).blackboard->template set<int>(  "number_recoveries",   recovery_count ); // NOLINT
           }
          
           std::string action_name_;
           typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
          
           // All ROS2 actions have a goal and a result
           typename ActionT::Goal goal_;
           bool goal_updated_{false};
           bool goal_result_available_{false};
           typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
           typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
          
           // To handle feedback from action server
           std::shared_ptr<const typename ActionT::Feedback> feedback_;
          
           // The node that will be used for any ROS operations
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          
           // The timeout value while waiting for response from a server when a
           // new action goal is sent or canceled
           std::chrono::milliseconds server_timeout_;
          
           // The timeout value for BT loop execution
           std::chrono::milliseconds bt_loop_duration_;
          
           // To track the action server acknowledgement when a new goal is sent
           std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
           future_goal_handle_;
           rclcpp::Time time_goal_sent_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_behavior_tree/behavior_tree_engine.hpp"
          #include "nav2_behavior_tree/ros_topic_logger.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/simple_action_server.hpp"
          
          namespace nav2_behavior_tree
          {
          /**
           * @class nav2_behavior_tree::BtActionServer
           * @brief An action server that uses behavior tree to execute an action
           */
          template<class ActionT>
      35  class BtActionServer
          {
          public:
           using ActionServer = nav2_util::SimpleActionServer<ActionT>;
          
           typedef std::function<bool (  typename ActionT::Goal::ConstSharedPtr )> OnGoalReceivedCallback;
           typedef std::function<void (   )> OnLoopCallback;
           typedef std::function<void (  typename ActionT::Goal::ConstSharedPtr )> OnPreemptCallback;
           typedef std::function<void (  typename ActionT::Result::SharedPtr,  
           nav2_behavior_tree::BtStatus )> OnCompletionCallback;
          
           /**
           * @brief A constructor for nav2_behavior_tree::BtActionServer class
           */
           explicit BtActionServer(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & action_name,  
           const std::vector<std::string> & plugin_lib_names,  
           const std::string & default_bt_xml_filename,  
           OnGoalReceivedCallback on_goal_received_callback,  
           OnLoopCallback on_loop_callback,  
           OnPreemptCallback on_preempt_callback,  
           OnCompletionCallback on_completion_callback );
          
           /**
           * @brief A destructor for nav2_behavior_tree::BtActionServer class
           */
           ~BtActionServer(   );
          
           /**
           * @brief Configures member variables
           * Initializes action server for,   builds behavior tree from xml file,  
           * and calls user-defined onConfigure.
           * @return bool true on SUCCESS and false on FAILURE
           */
           bool on_configure(   );
          
           /**
           * @brief Activates action server
           * @return bool true on SUCCESS and false on FAILURE
           */
           bool on_activate(   );
          
           /**
           * @brief Deactivates action server
           * @return bool true on SUCCESS and false on FAILURE
           */
           bool on_deactivate(   );
          
           /**
           * @brief Resets member variables
           * @return bool true on SUCCESS and false on FAILURE
           */
           bool on_cleanup(   );
          
           /**
           * @brief Replace current BT with another one
           * @param bt_xml_filename The file containing the new BT,   uses default filename if empty
           * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
           * if something went wrong,   and previous BT is maintained
           */
           bool loadBehaviorTree(  const std::string & bt_xml_filename = "" );
          
           /**
           * @brief Getter function for BT Blackboard
           * @return BT::Blackboard::Ptr Shared pointer to current BT blackboard
           */
           BT::Blackboard::Ptr getBlackboard(   ) const
           {
           return blackboard_;
           }
          
           /**
           * @brief Getter function for current BT XML filename
           * @return string Containing current BT XML filename
           */
     111   std::string getCurrentBTFilename(   ) const
           {
           return current_bt_xml_filename_;
           }
          
           /**
           * @brief Getter function for default BT XML filename
           * @return string Containing default BT XML filename
           */
     120   std::string getDefaultBTFilename(   ) const
           {
           return default_bt_xml_filename_;
           }
          
           /**
           * @brief Wrapper function to accept pending goal if a preempt has been requested
           * @return Shared pointer to pending action goal
           */
     129   const std::shared_ptr<const typename ActionT::Goal> acceptPendingGoal(   )
           {
           return action_server_->accept_pending_goal(   );
           }
          
           /**
           * @brief Wrapper function to terminate pending goal if a preempt has been requested
           */
     137   void terminatePendingGoal(   )
           {
           action_server_->terminate_pending_goal(   );
           }
          
           /**
           * @brief Wrapper function to get current goal
           * @return Shared pointer to current action goal
           */
     146   const std::shared_ptr<const typename ActionT::Goal> getCurrentGoal(   ) const
           {
           return action_server_->get_current_goal(   );
           }
          
           /**
           * @brief Wrapper function to get pending goal
           * @return Shared pointer to pending action goal
           */
     155   const std::shared_ptr<const typename ActionT::Goal> getPendingGoal(   ) const
           {
           return action_server_->get_pending_goal(   );
           }
          
           /**
           * @brief Wrapper function to publish action feedback
           */
     163   void publishFeedback(  typename std::shared_ptr<typename ActionT::Feedback> feedback )
           {
           action_server_->publish_feedback(  feedback );
           }
          
           /**
           * @brief Getter function for the current BT tree
           * @return BT::Tree Current behavior tree
           */
     172   const BT::Tree & getTree(   ) const
           {
           return tree_;
           }
          
           /**
           * @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes
           * by calling their halt(   ) implementation (  only for Async nodes that may return RUNNING )
           */
     181   void haltTree(   )
           {
           tree_.rootNode(   )->halt(   );
           }
          
          protected:
           /**
           * @brief Action server callback
           */
     190   void executeCallback(   );
          
           // Action name
           std::string action_name_;
          
           // Our action server implements the template action
           std::shared_ptr<ActionServer> action_server_;
          
           // Behavior Tree to be executed when goal is received
           BT::Tree tree_;
          
           // The blackboard shared by all of the nodes in the tree
           BT::Blackboard::Ptr blackboard_;
          
           // The XML file that cointains the Behavior Tree to create
           std::string current_bt_xml_filename_;
           std::string default_bt_xml_filename_;
          
           // The wrapper class for the BT functionality
           std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
          
           // Libraries to pull plugins (  BT Nodes ) from
           std::vector<std::string> plugin_lib_names_;
          
           // A regular,   non-spinning ROS node that we can use for calls to the action client
           rclcpp::Node::SharedPtr client_node_;
          
           // Parent node
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
          
           // Clock
           rclcpp::Clock::SharedPtr clock_;
          
           // Logger
     224   rclcpp::Logger logger_{rclcpp::get_logger(  "BtActionServer" )};
          
           // To publish BT logs
           std::unique_ptr<RosTopicLogger> topic_logger_;
          
           // Duration for each iteration of BT execution
           std::chrono::milliseconds bt_loop_duration_;
          
           // Default timeout value while waiting for response from a server
           std::chrono::milliseconds default_server_timeout_;
          
           // User-provided callbacks
           OnGoalReceivedCallback on_goal_received_callback_;
           OnLoopCallback on_loop_callback_;
           OnPreemptCallback on_preempt_callback_;
           OnCompletionCallback on_completion_callback_;
          };
          
          } // namespace nav2_behavior_tree
          
          #include <nav2_behavior_tree/bt_action_server_impl.hpp> // NOLINT(  build/include_order )
          #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
          
          #include <memory>
          #include <string>
          #include <fstream>
          #include <set>
          #include <exception>
          #include <vector>
          
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "nav2_behavior_tree/bt_action_server.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          namespace nav2_behavior_tree
          {
          
          template<class ActionT>
      33  BtActionServer<ActionT>::BtActionServer(  
      34   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      35   const std::string & action_name,  
      36   const std::vector<std::string> & plugin_lib_names,  
      37   const std::string & default_bt_xml_filename,  
      38   OnGoalReceivedCallback on_goal_received_callback,  
      39   OnLoopCallback on_loop_callback,  
      40   OnPreemptCallback on_preempt_callback,  
      41   OnCompletionCallback on_completion_callback )
          : action_name_(  action_name ),  
           default_bt_xml_filename_(  default_bt_xml_filename ),  
           plugin_lib_names_(  plugin_lib_names ),  
           node_(  parent ),  
           on_goal_received_callback_(  on_goal_received_callback ),  
           on_loop_callback_(  on_loop_callback ),  
           on_preempt_callback_(  on_preempt_callback ),  
           on_completion_callback_(  on_completion_callback )
          {
           auto node = node_.lock(   );
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
          
           // Declare this node's parameters
           if (  !node->has_parameter(  "bt_loop_duration" ) ) {
           node->declare_parameter(  "bt_loop_duration",   10 );
           }
           if (  !node->has_parameter(  "default_server_timeout" ) ) {
           node->declare_parameter(  "default_server_timeout",   20 );
           }
          }
          
          template<class ActionT>
      65  BtActionServer<ActionT>::~BtActionServer(   )
          {}
          
          template<class ActionT>
      69  bool BtActionServer<ActionT>::on_configure(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           // Name client node after action name
           std::string client_node_name = action_name_;
           std::replace(  client_node_name.begin(   ),   client_node_name.end(   ),   '/',   '_' );
           // Use suffix '_rclcpp_node' to keep parameter file consistency #1773
           auto options = rclcpp::NodeOptions(   ).arguments(  
           {"--ros-args",  
           "-r",  
           std::string(  "__node:=" ) +
           std::string(  node->get_name(   ) ) + "_" + client_node_name + "_rclcpp_node",  
           "--"} );
          
           // Support for handling the topic-based goal pose from rviz
           client_node_ = std::make_shared<rclcpp::Node>(  "_",   options );
          
           action_server_ = std::make_shared<ActionServer>(  
           node->get_node_base_interface(   ),  
           node->get_node_clock_interface(   ),  
           node->get_node_logging_interface(   ),  
           node->get_node_waitables_interface(   ),  
           action_name_,   std::bind(  &BtActionServer<ActionT>::executeCallback,   this ) );
          
           // Get parameters for BT timeouts
           int timeout;
           node->get_parameter(  "bt_loop_duration",   timeout );
           bt_loop_duration_ = std::chrono::milliseconds(  timeout );
           node->get_parameter(  "default_server_timeout",   timeout );
           default_server_timeout_ = std::chrono::milliseconds(  timeout );
          
           // Create the class that registers our custom nodes and executes the BT
           bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(  plugin_lib_names_ );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           blackboard_ = BT::Blackboard::create(   );
          
           // Put items on the blackboard
           blackboard_->set<rclcpp::Node::SharedPtr>(  "node",   client_node_ ); // NOLINT
           blackboard_->set<std::chrono::milliseconds>(  "server_timeout",   default_server_timeout_ ); // NOLINT
           blackboard_->set<std::chrono::milliseconds>(  "bt_loop_duration",   bt_loop_duration_ ); // NOLINT
          
           return true;
          }
          
          template<class ActionT>
     119  bool BtActionServer<ActionT>::on_activate(   )
          {
           if (  !loadBehaviorTree(  default_bt_xml_filename_ ) ) {
           RCLCPP_ERROR(  logger_,   "Error loading XML file: %s",   default_bt_xml_filename_.c_str(   ) );
           return false;
           }
           action_server_->activate(   );
           return true;
          }
          
          template<class ActionT>
     130  bool BtActionServer<ActionT>::on_deactivate(   )
          {
           action_server_->deactivate(   );
           return true;
          }
          
          template<class ActionT>
     137  bool BtActionServer<ActionT>::on_cleanup(   )
          {
           client_node_.reset(   );
           action_server_.reset(   );
           topic_logger_.reset(   );
           plugin_lib_names_.clear(   );
           current_bt_xml_filename_.clear(   );
           blackboard_.reset(   );
           bt_->haltAllActions(  tree_.rootNode(   ) );
           bt_.reset(   );
           return true;
          }
          
          template<class ActionT>
     151  bool BtActionServer<ActionT>::loadBehaviorTree(  const std::string & bt_xml_filename )
          {
           // Empty filename is default for backward compatibility
           auto filename = bt_xml_filename.empty(   ) ? default_bt_xml_filename_ : bt_xml_filename;
          
           // Use previous BT if it is the existing one
           if (  current_bt_xml_filename_ == filename ) {
           RCLCPP_DEBUG(  logger_,   "BT will not be reloaded as the given xml is already loaded" );
           return true;
           }
          
           // Read the input BT XML from the specified file into a string
           std::ifstream xml_file(  filename );
          
           if (  !xml_file.good(   ) ) {
           RCLCPP_ERROR(  logger_,   "Couldn't open input XML file: %s",   filename.c_str(   ) );
           return false;
           }
          
           auto xml_string = std::string(  
           std::istreambuf_iterator<char>(  xml_file ),  
           std::istreambuf_iterator<char>(   ) );
          
           // Create the Behavior Tree from the XML input
           try {
           tree_ = bt_->createTreeFromText(  xml_string,   blackboard_ );
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  logger_,   "Exception when loading BT: %s",   e.what(   ) );
           return false;
           }
          
           topic_logger_ = std::make_unique<RosTopicLogger>(  client_node_,   tree_ );
          
           current_bt_xml_filename_ = filename;
           return true;
          }
          
          template<class ActionT>
     189  void BtActionServer<ActionT>::executeCallback(   )
          {
           if (  !on_goal_received_callback_(  action_server_->get_current_goal(   ) ) ) {
           action_server_->terminate_current(   );
           return;
           }
          
           auto is_canceling = [&](   ) {
           if (  action_server_ == nullptr ) {
           RCLCPP_DEBUG(  logger_,   "Action server unavailable. Canceling." );
           return true;
           }
           if (  !action_server_->is_server_active(   ) ) {
           RCLCPP_DEBUG(  logger_,   "Action server is inactive. Canceling." );
           return true;
           }
           return action_server_->is_cancel_requested(   );
           };
          
           auto on_loop = [&](   ) {
           if (  action_server_->is_preempt_requested(   ) && on_preempt_callback_ ) {
           on_preempt_callback_(  action_server_->get_pending_goal(   ) );
           }
           topic_logger_->flush(   );
           on_loop_callback_(   );
           };
          
           // Execute the BT that was previously created in the configure step
           nav2_behavior_tree::BtStatus rc = bt_->run(  &tree_,   on_loop,   is_canceling,   bt_loop_duration_ );
          
           // Make sure that the Bt is not in a running state from a previous execution
           // note: if all the ControlNodes are implemented correctly,   this is not needed.
           bt_->haltAllActions(  tree_.rootNode(   ) );
          
           // Give server an opportunity to populate the result message or simple give
           // an indication that the action is complete.
           auto result = std::make_shared<typename ActionT::Result>(   );
           on_completion_callback_(  result,   rc );
          
           switch (  rc ) {
           case nav2_behavior_tree::BtStatus::SUCCEEDED:
           RCLCPP_INFO(  logger_,   "Goal succeeded" );
           action_server_->succeeded_current(  result );
           break;
          
           case nav2_behavior_tree::BtStatus::FAILED:
           RCLCPP_ERROR(  logger_,   "Goal failed" );
           action_server_->terminate_current(  result );
           break;
          
           case nav2_behavior_tree::BtStatus::CANCELED:
           RCLCPP_INFO(  logger_,   "Goal canceled" );
           action_server_->terminate_all(  result );
           break;
           }
          }
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
          
          #include <memory>
          #include <string>
          #include <chrono>
          
          #include "behaviortree_cpp_v3/action_node.h"
          #include "nav2_util/node_utils.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_behavior_tree/bt_conversions.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief Abstract class representing an action for cancelling BT node
           * @tparam ActionT Type of action
           */
          template<class ActionT>
      35  class BtCancelActionNode : public BT::ActionNodeBase
          {
          public:
           /**
           * @brief A nav2_behavior_tree::BtCancelActionNode constructor
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      44   BtCancelActionNode(  
      45   const std::string & xml_tag_name,  
      46   const std::string & action_name,  
      47   const BT::NodeConfiguration & conf )
           : BT::ActionNodeBase(  xml_tag_name,   conf ),   action_name_(  action_name )
           {
           node_ = config(   ).blackboard->template get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           // Get the required items from the blackboard
           server_timeout_ =
           config(   ).blackboard->template get<std::chrono::milliseconds>(  "server_timeout" );
           getInput<std::chrono::milliseconds>(  "server_timeout",   server_timeout_ );
          
           std::string remapped_action_name;
           if (  getInput(  "server_name",   remapped_action_name ) ) {
           action_name_ = remapped_action_name;
           }
           createActionClient(  action_name_ );
          
           // Give the derive class a chance to do any initialization
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "\"%s\" BtCancelActionNode initialized",  
           xml_tag_name.c_str(   ) );
           }
          
           BtCancelActionNode(   ) = delete;
          
           virtual ~BtCancelActionNode(   )
           {
           }
          
           /**
           * @brief Create instance of an action client
           * @param action_name Action name to create client for
           */
           void createActionClient(  const std::string & action_name )
           {
           // Now that we have the ROS node to use,   create the action client for this BT action
           action_client_ = rclcpp_action::create_client<ActionT>(  node_,   action_name,   callback_group_ );
          
           // Make sure the server is actually there before continuing
      89   RCLCPP_DEBUG(  node_->get_logger(   ),   "Waiting for \"%s\" action server",   action_name.c_str(   ) );
      90   action_client_->wait_for_action_server(   );
           }
          
           /**
           * @brief Any subclass of BtCancelActionNode that accepts parameters must provide a
           * providedPorts method and call providedBasicPorts in it.
           * @param addition Additional ports to add to BT port list
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      99   static BT::PortsList providedBasicPorts(  BT::PortsList addition )
           {
           BT::PortsList basic = {
           BT::InputPort<std::string>(  "server_name",   "Action server name" ),  
           BT::InputPort<std::chrono::milliseconds>(  "server_timeout" )
           };
           basic.insert(  addition.begin(   ),   addition.end(   ) );
          
           return basic;
           }
          
     110   void halt(   )
           {
           }
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
     118   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  {} );
           }
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
     127   BT::NodeStatus tick(   ) override
           {
           // setting the status to RUNNING to notify the BT Loggers (  if any )
           setStatus(  BT::NodeStatus::RUNNING );
          
           // Cancel all the goals specified before 10ms from current time
           // to avoid async communication error
          
           rclcpp::Time goal_expiry_time = node_->now(   ) - std::chrono::milliseconds(  10 );
          
           auto future_cancel = action_client_->async_cancel_goals_before(  goal_expiry_time );
          
           if (  callback_group_executor_.spin_until_future_complete(  future_cancel,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Failed to cancel the action server for %s",   action_name_.c_str(   ) );
           return BT::NodeStatus::FAILURE;
           }
           return BT::NodeStatus::SUCCESS;
           }
          
          protected:
           std::string action_name_;
           typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
          
           // The node that will be used for any ROS operations
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          
           // The timeout value while waiting for response from a server when a
           // new action goal is canceled
           std::chrono::milliseconds server_timeout_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
          
          #include <string>
          
          #include "rclcpp/time.hpp"
          #include "behaviortree_cpp_v3/behavior_tree.h"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          namespace BT
          {
          
          // The follow templates are required when using these types as parameters
          // in our BT XML files. They parse the strings in the XML into their corresponding
          // data type.
          
          /**
           * @brief Parse XML string to geometry_msgs::msg::Point
           * @param key XML string
           * @return geometry_msgs::msg::Point
           */
          template<>
      39  inline geometry_msgs::msg::Point convertFromString(  const StringView key )
          {
           // three real numbers separated by semicolons
           auto parts = BT::splitString(  key,   ';' );
           if (  parts.size(   ) != 3 ) {
           throw std::runtime_error(  "invalid number of fields for point attribute )" );
           } else {
           geometry_msgs::msg::Point position;
           position.x = BT::convertFromString<double>(  parts[0] );
           position.y = BT::convertFromString<double>(  parts[1] );
           position.z = BT::convertFromString<double>(  parts[2] );
           return position;
           }
          }
          
          /**
           * @brief Parse XML string to geometry_msgs::msg::Quaternion
           * @param key XML string
           * @return geometry_msgs::msg::Quaternion
           */
          template<>
      60  inline geometry_msgs::msg::Quaternion convertFromString(  const StringView key )
          {
           // four real numbers separated by semicolons
           auto parts = BT::splitString(  key,   ';' );
           if (  parts.size(   ) != 4 ) {
           throw std::runtime_error(  "invalid number of fields for orientation attribute )" );
           } else {
           geometry_msgs::msg::Quaternion orientation;
           orientation.x = BT::convertFromString<double>(  parts[0] );
           orientation.y = BT::convertFromString<double>(  parts[1] );
           orientation.z = BT::convertFromString<double>(  parts[2] );
           orientation.w = BT::convertFromString<double>(  parts[3] );
           return orientation;
           }
          }
          
          /**
           * @brief Parse XML string to geometry_msgs::msg::PoseStamped
           * @param key XML string
           * @return geometry_msgs::msg::PoseStamped
           */
          template<>
      82  inline geometry_msgs::msg::PoseStamped convertFromString(  const StringView key )
          {
           // 7 real numbers separated by semicolons
           auto parts = BT::splitString(  key,   ';' );
           if (  parts.size(   ) != 9 ) {
           throw std::runtime_error(  "invalid number of fields for PoseStamped attribute )" );
           } else {
           geometry_msgs::msg::PoseStamped pose_stamped;
           pose_stamped.header.stamp = rclcpp::Time(  BT::convertFromString<int64_t>(  parts[0] ) );
           pose_stamped.header.frame_id = BT::convertFromString<std::string>(  parts[1] );
           pose_stamped.pose.position.x = BT::convertFromString<double>(  parts[2] );
           pose_stamped.pose.position.y = BT::convertFromString<double>(  parts[3] );
           pose_stamped.pose.position.z = BT::convertFromString<double>(  parts[4] );
           pose_stamped.pose.orientation.x = BT::convertFromString<double>(  parts[5] );
           pose_stamped.pose.orientation.y = BT::convertFromString<double>(  parts[6] );
           pose_stamped.pose.orientation.z = BT::convertFromString<double>(  parts[7] );
           pose_stamped.pose.orientation.w = BT::convertFromString<double>(  parts[8] );
           return pose_stamped;
           }
          }
          
          /**
           * @brief Parse XML string to std::chrono::milliseconds
           * @param key XML string
           * @return std::chrono::milliseconds
           */
          template<>
     109  inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(  const StringView key )
          {
           return std::chrono::milliseconds(  std::stoul(  key.data(   ) ) );
          }
          
          } // namespace BT
          
          #endif // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
          
          #include <string>
          #include <memory>
          
          #include "behaviortree_cpp_v3/action_node.h"
          #include "nav2_util/node_utils.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_behavior_tree/bt_conversions.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief Abstract class representing a service based BT node
           * @tparam ServiceT Type of service
           */
          template<class ServiceT>
      34  class BtServiceNode : public BT::ActionNodeBase
          {
          public:
           /**
           * @brief A nav2_behavior_tree::BtServiceNode constructor
           * @param service_node_name Service name this node creates a client for
           * @param conf BT node configuration
           */
      42   BtServiceNode(  
      43   const std::string & service_node_name,  
      44   const BT::NodeConfiguration & conf )
           : BT::ActionNodeBase(  service_node_name,   conf ),   service_node_name_(  service_node_name )
           {
           node_ = config(   ).blackboard->template get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           // Get the required items from the blackboard
           bt_loop_duration_ =
           config(   ).blackboard->template get<std::chrono::milliseconds>(  "bt_loop_duration" );
           server_timeout_ =
           config(   ).blackboard->template get<std::chrono::milliseconds>(  "server_timeout" );
           getInput<std::chrono::milliseconds>(  "server_timeout",   server_timeout_ );
          
           // Now that we have node_ to use,   create the service client for this BT service
           getInput(  "service_name",   service_name_ );
           service_client_ = node_->create_client<ServiceT>(  
           service_name_,  
           rmw_qos_profile_services_default,  
           callback_group_ );
          
           // Make a request for the service without parameter
           request_ = std::make_shared<typename ServiceT::Request>(   );
          
           // Make sure the server is actually there before continuing
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Waiting for \"%s\" service",  
           service_name_.c_str(   ) );
           service_client_->wait_for_service(   );
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "\"%s\" BtServiceNode initialized",  
           service_node_name_.c_str(   ) );
           }
          
           BtServiceNode(   ) = delete;
          
           virtual ~BtServiceNode(   )
           {
           }
          
           /**
           * @brief Any subclass of BtServiceNode that accepts parameters must provide a
           * providedPorts method and call providedBasicPorts in it.
           * @param addition Additional ports to add to BT port list
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedBasicPorts(  BT::PortsList addition )
           {
           BT::PortsList basic = {
           BT::InputPort<std::string>(  "service_name",   "please_set_service_name_in_BT_Node" ),  
           BT::InputPort<std::chrono::milliseconds>(  "server_timeout" )
           };
      99   basic.insert(  addition.begin(   ),   addition.end(   ) );
          
           return basic;
           }
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
     108   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  {} );
           }
          
           /**
           * @brief The main override required by a BT service
           * @return BT::NodeStatus Status of tick execution
           */
     117   BT::NodeStatus tick(   ) override
           {
           if (  !request_sent_ ) {
           on_tick(   );
           future_result_ = service_client_->async_send_request(  request_ ).share(   );
           sent_time_ = node_->now(   );
           request_sent_ = true;
           }
           return check_future(   );
           }
          
           /**
           * @brief The other (  optional ) override required by a BT service.
           */
     131   void halt(   ) override
           {
           request_sent_ = false;
           setStatus(  BT::NodeStatus::IDLE );
           }
          
           /**
           * @brief Function to perform some user-defined operation on tick
           * Fill in service request with information if necessary
           */
           virtual void on_tick(   )
           {
           }
          
           /**
           * @brief Function to perform some user-defined operation upon successful
           * completion of the service. Could put a value on the blackboard.
           * @param response can be used to get the result of the service call in the BT Node.
           * @return BT::NodeStatus Returns SUCCESS by default,   user may override to return another value
           */
           virtual BT::NodeStatus on_completion(  std::shared_ptr<typename ServiceT::Response>/*response*/ )
           {
           return BT::NodeStatus::SUCCESS;
           }
          
           /**
           * @brief Check the future and decide the status of BT
           * @return BT::NodeStatus SUCCESS if future complete before timeout,   FAILURE otherwise
           */
           virtual BT::NodeStatus check_future(   )
           {
           auto elapsed = (  node_->now(   ) - sent_time_ ).to_chrono<std::chrono::milliseconds>(   );
           auto remaining = server_timeout_ - elapsed;
          
           if (  remaining > std::chrono::milliseconds(  0 ) ) {
           auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
          
           rclcpp::FutureReturnCode rc;
           rc = callback_group_executor_.spin_until_future_complete(  future_result_,   server_timeout_ );
           if (  rc == rclcpp::FutureReturnCode::SUCCESS ) {
           request_sent_ = false;
           BT::NodeStatus status = on_completion(  future_result_.get(   ) );
           return status;
           }
          
           if (  rc == rclcpp::FutureReturnCode::TIMEOUT ) {
           on_wait_for_result(   );
           elapsed = (  node_->now(   ) - sent_time_ ).to_chrono<std::chrono::milliseconds>(   );
           if (  elapsed < server_timeout_ ) {
           return BT::NodeStatus::RUNNING;
           }
           }
           }
          
           RCLCPP_WARN(  
           node_->get_logger(   ),  
           "Node timed out while executing service call to %s.",   service_name_.c_str(   ) );
           request_sent_ = false;
           return BT::NodeStatus::FAILURE;
           }
          
           /**
           * @brief Function to perform some user-defined operation after a timeout waiting
           * for a result that hasn't been received yet
           */
           virtual void on_wait_for_result(   )
           {
           }
          
          protected:
           /**
           * @brief Function to increment recovery count on blackboard if this node wraps a recovery
           */
           void increment_recovery_count(   )
           {
           int recovery_count = 0;
           config(   ).blackboard->template get<int>(  "number_recoveries",   recovery_count ); // NOLINT
           recovery_count += 1;
           config(   ).blackboard->template set<int>(  "number_recoveries",   recovery_count ); // NOLINT
           }
          
           std::string service_name_,   service_node_name_;
           typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
           std::shared_ptr<typename ServiceT::Request> request_;
          
           // The node that will be used for any ROS operations
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          
           // The timeout value while to use in the tick loop while waiting for
           // a result from the server
           std::chrono::milliseconds server_timeout_;
          
           // The timeout value for BT loop execution
           std::chrono::milliseconds bt_loop_duration_;
          
           // To track the server response when a new request is sent
           std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
           bool request_sent_{false};
           rclcpp::Time sent_time_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_action_node.hpp"
          #include "nav2_msgs/action/back_up.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
           */
      29  class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::BackUpAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      38   BackUpAction(  
      39   const std::string & xml_tag_name,  
      40   const std::string & action_name,  
      41   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<double>(  "backup_dist",   0.15,   "Distance to backup" ),  
           BT::InputPort<double>(  "backup_speed",   0.025,   "Speed at which to backup" ),  
           BT::InputPort<double>(  "time_allowance",   10.0,   "Allowed time for reversing" )
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_msgs/action/back_up.hpp"
          
          #include "nav2_behavior_tree/bt_cancel_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
           */
      31  class BackUpCancel : public BtCancelActionNode<nav2_msgs::action::BackUp>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::BackUpAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   BackUpCancel(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp

          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_service_node.hpp"
          #include "nav2_msgs/srv/clear_entire_costmap.hpp"
          #include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
          #include "nav2_msgs/srv/clear_costmap_except_region.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap
           */
      31  class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ClearEntireCostmapService
           * @param service_node_name Service name this node creates a client for
           * @param conf BT node configuration
           */
      39   ClearEntireCostmapService(  
      40   const std::string & service_node_name,  
      41   const BT::NodeConfiguration & conf );
          
           /**
           * @brief The main override required by a BT service
           * @return BT::NodeStatus Status of tick execution
           */
           void on_tick(   ) override;
          };
          
          /**
           * @brief A nav2_behavior_tree::BtServiceNode class that
           * wraps nav2_msgs::srv::ClearCostmapExceptRegion
           */
      54  class ClearCostmapExceptRegionService
      55   : public BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ClearCostmapExceptRegionService
           * @param service_node_name Service name this node creates a client for
           * @param conf BT node configuration
           */
      63   ClearCostmapExceptRegionService(  
      64   const std::string & service_node_name,  
      65   const BT::NodeConfiguration & conf );
          
           /**
           * @brief The main override required by a BT service
           * @return BT::NodeStatus Status of tick execution
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<double>(  
           "reset_distance",   1,  
           "Distance from the robot above which obstacles are cleared" )
           } );
           }
          };
          
          /**
           * @brief A nav2_behavior_tree::BtServiceNode class that
           * wraps nav2_msgs::srv::ClearCostmapAroundRobot
           */
          class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ClearCostmapAroundRobotService
           * @param service_node_name Service name this node creates a client for
           * @param conf BT node configuration
           */
           ClearCostmapAroundRobotService(  
           const std::string & service_node_name,  
           const BT::NodeConfiguration & conf );
          
           /**
           * @brief The main override required by a BT service
           * @return BT::NodeStatus Status of tick execution
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<double>(  
           "reset_distance",   1,  
           "Distance from the robot under which obstacles are cleared" )
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
          
          #include <string>
          #include <vector>
          
          #include "nav2_msgs/action/compute_path_through_poses.hpp"
          #include "nav_msgs/msg/path.h"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathThroughPoses
           */
      32  class ComputePathThroughPosesAction
      33   : public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      42   ComputePathThroughPosesAction(  
      43   const std::string & xml_tag_name,  
      44   const std::string & action_name,  
      45   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Function to perform some user-defined operation upon successful completion of the action
           */
           BT::NodeStatus on_success(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::OutputPort<nav_msgs::msg::Path>(  "path",   "Path created by ComputePathThroughPoses node" ),  
           BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(  
           "goals",  
           "Destinations to plan through" ),  
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  
           "start",   "Start pose of the path if overriding current robot pose" ),  
           BT::InputPort<std::string>(  
           "planner_id",   "",  
           "Mapped name to the planner plugin type to use" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_msgs/action/compute_path_to_pose.hpp"
          #include "nav_msgs/msg/path.h"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose
           */
      30  class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      39   ComputePathToPoseAction(  
      40   const std::string & xml_tag_name,  
      41   const std::string & action_name,  
      42   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Function to perform some user-defined operation upon successful completion of the action
           */
           BT::NodeStatus on_success(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::OutputPort<nav_msgs::msg::Path>(  "path",   "Path created by ComputePathToPose node" ),  
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  "goal",   "Destination to plan to" ),  
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  
           "start",   "Start pose of the path if overriding current robot pose" ),  
           BT::InputPort<std::string>(  
           "planner_id",   "",  
           "Mapped name to the planner plugin type to use" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_cancel_node.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_msgs/action/follow_path.hpp"
          
          #include "nav2_behavior_tree/bt_cancel_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
           */
      31  class ControllerCancel : public BtCancelActionNode<nav2_msgs::action::FollowPath>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::FollowPathAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   ControllerCancel(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "behaviortree_cpp_v3/action_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief The ControllerSelector behavior is used to switch the controller
           * that will be used by the controller server. It subscribes to a topic "controller_selector"
           * to get the decision about what controller must be used. It is usually used before of
           * the FollowPath. The selected_controller output port is passed to controller_id
           * input port of the FollowPath
           */
      38  class ControllerSelector : public BT::SyncActionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ControllerSelector
           *
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      47   ControllerSelector(  
      48   const std::string & xml_tag_name,  
      49   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      55   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<std::string>(  
           "default_controller",  
           "the default controller to use if there is not any external topic message received." ),  
          
           BT::InputPort<std::string>(  
           "topic_name",  
           "controller_selector",  
           "the input topic name to select the controller" ),  
          
           BT::OutputPort<std::string>(  
           "selected_controller",  
           "Selected controller by subscription" )
           };
           }
          
          private:
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief callback function for the controller_selector topic
           *
           * @param msg the message with the id of the controller_selector
           */
           void callbackControllerSelect(  const std_msgs::msg::String::SharedPtr msg );
          
           rclcpp::Subscription<std_msgs::msg::String>::SharedPtr controller_selector_sub_;
          
           std::string last_selected_controller_;
          
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          
           std::string topic_name_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_action_node.hpp"
          #include "nav2_msgs/action/drive_on_heading.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
           */
      29  class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      38   DriveOnHeadingAction(  
      39   const std::string & xml_tag_name,  
      40   const std::string & action_name,  
      41   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      47   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<double>(  "dist_to_travel",   0.15,   "Distance to travel" ),  
           BT::InputPort<double>(  "speed",   0.025,   "Speed at which to travel" ),  
           BT::InputPort<double>(  "time_allowance",   10.0,   "Allowed time for driving on heading" )
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_msgs/action/drive_on_heading.hpp"
          
          #include "nav2_behavior_tree/bt_cancel_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
           */
      31  class DriveOnHeadingCancel : public BtCancelActionNode<nav2_msgs::action::DriveOnHeading>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::DriveOnHeadingCancel
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   DriveOnHeadingCancel(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
          
          #include <string>
          #include <memory>
          
          #include "nav2_msgs/action/follow_path.hpp"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
           */
      30  class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::FollowPathAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      39   FollowPathAction(  
      40   const std::string & xml_tag_name,  
      41   const std::string & action_name,  
      42   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Function to perform some user-defined operation after a timeout
           * waiting for a result that hasn't been received yet
           * @param feedback shared_ptr to latest feedback message
           */
           void on_wait_for_result(  
           std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<nav_msgs::msg::Path>(  "path",   "Path to follow" ),  
           BT::InputPort<std::string>(  "controller_id",   "" ),  
           BT::InputPort<std::string>(  "goal_checker_id",   "" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "behaviortree_cpp_v3/action_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief The GoalCheckerSelector behavior is used to switch the goal checker
           * of the controller server. It subscribes to a topic "goal_checker_selector"
           * to get the decision about what goal_checker must be used. It is usually used before of
           * the FollowPath. The selected_goal_checker output port is passed to goal_checker_id
           * input port of the FollowPath
           */
      38  class GoalCheckerSelector : public BT::SyncActionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GoalCheckerSelector
           *
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      47   GoalCheckerSelector(  
      48   const std::string & xml_tag_name,  
      49   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      55   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<std::string>(  
           "default_goal_checker",  
           "the default goal_checker to use if there is not any external topic message received." ),  
          
           BT::InputPort<std::string>(  
           "topic_name",  
           "goal_checker_selector",  
           "the input topic name to select the goal_checker" ),  
          
           BT::OutputPort<std::string>(  
           "selected_goal_checker",  
           "Selected goal_checker by subscription" )
           };
           }
          
          private:
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief callback function for the goal_checker_selector topic
           *
           * @param msg the message with the id of the goal_checker_selector
           */
           void callbackGoalCheckerSelect(  const std_msgs::msg::String::SharedPtr msg );
          
           rclcpp::Subscription<std_msgs::msg::String>::SharedPtr goal_checker_selector_sub_;
          
           std::string last_selected_goal_checker_;
          
           rclcpp::Node::SharedPtr node_;
          
           std::string topic_name_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
          
          #include <string>
          
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "nav2_msgs/action/navigate_through_poses.hpp"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateThroughPoses
           */
      31  class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   NavigateThroughPosesAction(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  "goals",   "Destinations to plan through" ),  
           BT::InputPort<std::string>(  "behavior_tree",   "Behavior tree to run" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
          
          #include <string>
          
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose
           */
      31  class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   NavigateToPoseAction(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  "goal",   "Destination to plan to" ),  
           BT::InputPort<std::string>(  "behavior_tree",   "Behavior tree to run" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "behaviortree_cpp_v3/action_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief The PlannerSelector behavior is used to switch the planner
           * that will be used by the planner server. It subscribes to a topic "planner_selector"
           * to get the decision about what planner must be used. It is usually used before of
           * the ComputePathToPoseAction. The selected_planner output port is passed to planner_id
           * input port of the ComputePathToPoseAction
           */
      38  class PlannerSelector : public BT::SyncActionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::PlannerSelector
           *
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      47   PlannerSelector(  
      48   const std::string & xml_tag_name,  
      49   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      55   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<std::string>(  
           "default_planner",  
           "the default planner to use if there is not any external topic message received." ),  
          
           BT::InputPort<std::string>(  
           "topic_name",  
           "planner_selector",  
           "the input topic name to select the planner" ),  
          
           BT::OutputPort<std::string>(  
           "selected_planner",  
           "Selected planner by subscription" )
           };
           }
          
          private:
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief callback function for the planner_selector topic
           *
           * @param msg the message with the id of the planner_selector
           */
           void callbackPlannerSelect(  const std_msgs::msg::String::SharedPtr msg );
          
          
           rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planner_selector_sub_;
          
           std::string last_selected_planner_;
          
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          
           std::string topic_name_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_service_node.hpp"
          #include "std_srvs/srv/empty.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty
           */
      29  class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::ReinitializeGlobalLocalizationService
           * @param service_node_name Service name this node creates a client for
           * @param conf BT node configuration
           */
      37   ReinitializeGlobalLocalizationService(  
      38   const std::string & service_node_name,  
      39   const BT::NodeConfiguration & conf );
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
          
          #include <vector>
          #include <memory>
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "behaviortree_cpp_v3/action_node.h"
          
          namespace nav2_behavior_tree
          {
          
      30  class RemovePassedGoals : public BT::ActionNodeBase
          {
          public:
           typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
          
      35   RemovePassedGoals(  
      36   const std::string & xml_tag_name,  
      37   const BT::NodeConfiguration & conf );
          
          
      40   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<Goals>(  "input_goals",   "Original goals to remove viapoints from" ),  
           BT::OutputPort<Goals>(  "output_goals",   "Goals with passed viapoints removed" ),  
           BT::InputPort<double>(  "radius",   0.5,   "radius to goal for it to be considered for removal" ),  
           BT::InputPort<std::string>(  "global_frame",   std::string(  "map" ),   "Global frame" ),  
           BT::InputPort<std::string>(  "robot_base_frame",   std::string(  "base_link" ),   "Robot base frame" ),  
           };
           }
          
          private:
           void halt(   ) override {}
      53   BT::NodeStatus tick(   ) override;
          
           double viapoint_achieved_radius_;
           std::string robot_base_frame_,   global_frame_;
           double transform_tolerance_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp

          // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_msgs/action/smooth_path.hpp"
          #include "nav_msgs/msg/path.h"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::SmoothPath
           */
      31  class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::SmoothPath>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::SmoothPathAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   SmoothPathAction(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Function to perform some user-defined operation upon successful completion of the action
           */
           BT::NodeStatus on_success(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::OutputPort<nav_msgs::msg::Path>(  
           "smoothed_path",  
           "Path smoothed by SmootherServer node" ),  
           BT::OutputPort<double>(  "smoothing_duration",   "Time taken to smooth path" ),  
           BT::OutputPort<bool>(  
           "was_completed",   "True if smoothing was not interrupted by time limit" ),  
           BT::InputPort<nav_msgs::msg::Path>(  "unsmoothed_path",   "Path to be smoothed" ),  
           BT::InputPort<double>(  "max_smoothing_duration",   3.0,   "Maximum smoothing duration" ),  
           BT::InputPort<bool>(  
           "check_for_collisions",   false,  
           "If true collision check will be performed after smoothing" ),  
           BT::InputPort<std::string>(  "smoother_id",   "" ),  
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_action_node.hpp"
          #include "nav2_msgs/action/spin.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin
           */
      29  class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::SpinAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      38   SpinAction(  
      39   const std::string & xml_tag_name,  
      40   const std::string & action_name,  
      41   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<double>(  "spin_dist",   1.57,   "Spin distance" ),  
           BT::InputPort<double>(  "time_allowance",   10.0,   "Allowed time for spinning" ),  
           BT::InputPort<bool>(  "is_recovery",   true,   "True if recovery" )
           } );
           }
          
          private:
           bool is_recovery_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_msgs/action/spin.hpp"
          
          #include "nav2_behavior_tree/bt_cancel_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
           */
      31  class SpinCancel : public BtCancelActionNode<nav2_msgs::action::Spin>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::WaitAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   SpinCancel(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          
          #include "behaviortree_cpp_v3/action_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ActionNodeBase to shorten path by some distance
           */
      32  class TruncatePath : public BT::ActionNodeBase
          {
          public:
           /**
           * @brief A nav2_behavior_tree::TruncatePath constructor
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      40   TruncatePath(  
      41   const std::string & xml_tag_name,  
      42   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      48   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<nav_msgs::msg::Path>(  "input_path",   "Original Path" ),  
           BT::OutputPort<nav_msgs::msg::Path>(  "output_path",   "Path truncated to a certain distance" ),  
           BT::InputPort<double>(  "distance",   1.0,   "distance" ),  
           };
           }
          
          private:
           /**
           * @brief The other (  optional ) override required by a BT action.
           */
           void halt(   ) override {}
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
      67   BT::NodeStatus tick(   ) override;
          
           double distance_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
          
          #include <memory>
          #include <string>
          #include <limits>
          
          #include "nav_msgs/msg/path.hpp"
          
          #include "behaviortree_cpp_v3/action_node.h"
          #include "tf2_ros/buffer.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ActionNodeBase to shorten path to some distance around robot
           */
      34  class TruncatePathLocal : public BT::ActionNodeBase
          {
          public:
           /**
           * @brief A nav2_behavior_tree::TruncatePathLocal constructor
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      42   TruncatePathLocal(  
      43   const std::string & xml_tag_name,  
      44   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      50   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<nav_msgs::msg::Path>(  "input_path",   "Original Path" ),  
           BT::OutputPort<nav_msgs::msg::Path>(  
           "output_path",   "Path truncated to a certain distance around robot" ),  
           BT::InputPort<double>(  
           "distance_forward",   8.0,  
           "Distance in forward direction" ),  
           BT::InputPort<double>(  
           "distance_backward",   4.0,  
           "Distance in backward direction" ),  
           BT::InputPort<std::string>(  
           "robot_frame",   "base_link",  
           "Robot base frame id" ),  
           BT::InputPort<double>(  
           "transform_tolerance",   0.2,  
           "Transform lookup tolerance" ),  
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  
           "pose",   "Manually specified pose to be used"
           "if overriding current robot pose" ),  
           BT::InputPort<double>(  
           "angular_distance_weight",   0.0,  
           "Weight of angular distance relative to positional distance when finding which path "
           "pose is closest to robot. Not applicable on paths without orientations assigned" ),  
           BT::InputPort<double>(  
           "max_robot_pose_search_dist",   std::numeric_limits<double>::infinity(   ),  
           "Maximum forward integrated distance along the path (  starting from the last detected pose ) "
           "to bound the search for the closest pose to the robot. When set to infinity (  default ),   "
           "whole path is searched every time" ),  
           };
           }
          
          private:
           /**
           * @brief The other (  optional ) override required by a BT action.
           */
           void halt(   ) override {}
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
      93   BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Get either specified input pose or robot pose in path frame
           * @param path_frame_id Frame ID of path
           * @param pose Output pose
           * @return True if succeeded
           */
     101   bool getRobotPose(  std::string path_frame_id,   geometry_msgs::msg::PoseStamped & pose );
          
           /**
           * @brief A custom pose distance method which takes angular distance into account
           * in addition to spatial distance (  to improve picking a correct pose near cusps and loops )
           * @param pose1 Distance is computed between this pose and pose2
           * @param pose2 Distance is computed between this pose and pose1
           * @param angular_distance_weight Weight of angular distance relative to spatial distance
           * (  1.0 means that 1 radian of angular distance corresponds to 1 meter of spatial distance )
           */
     111   static double poseDistance(  
           const geometry_msgs::msg::PoseStamped & pose1,  
           const geometry_msgs::msg::PoseStamped & pose2,  
           const double angular_distance_weight );
          
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
          
           nav_msgs::msg::Path path_;
           nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp

          // Copyright (  c ) 2018 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
          
          #include <string>
          
          #include "nav2_behavior_tree/bt_action_node.hpp"
          #include "nav2_msgs/action/wait.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
           */
      29  class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::WaitAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      38   WaitAction(  
      39   const std::string & xml_tag_name,  
      40   const std::string & action_name,  
      41   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Function to perform some user-defined operation on tick
           */
           void on_tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           BT::InputPort<int>(  "wait_duration",   1,   "Wait time" )
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_msgs/action/wait.hpp"
          
          #include "nav2_behavior_tree/bt_cancel_action_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
           */
      31  class WaitCancel : public BtCancelActionNode<nav2_msgs::action::Wait>
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::WaitAction
           * @param xml_tag_name Name for the XML tag for this node
           * @param action_name Action name this node creates a client for
           * @param conf BT node configuration
           */
      40   WaitCancel(  
      41   const std::string & xml_tag_name,  
      42   const std::string & action_name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  
           {
           } );
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
          
          #include <string>
          #include <memory>
          
          #include "behaviortree_cpp_v3/condition_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "tf2_ros/buffer.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS every time the robot
           * travels a specified distance and FAILURE otherwise
           */
      35  class DistanceTraveledCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::DistanceTraveledCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      43   DistanceTraveledCondition(  
      44   const std::string & condition_name,  
      45   const BT::NodeConfiguration & conf );
          
           DistanceTraveledCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "distance",   1.0,   "Distance" ),  
           BT::InputPort<std::string>(  "global_frame",   std::string(  "map" ),   "Global frame" ),  
           BT::InputPort<std::string>(  "robot_base_frame",   std::string(  "base_link" ),   "Robot base frame" )
           };
           }
          
          private:
           rclcpp::Node::SharedPtr node_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           geometry_msgs::msg::PoseStamped start_pose_;
          
           double distance_;
           double transform_tolerance_;
           std::string global_frame_;
           std::string robot_base_frame_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

          // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
          
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          
          namespace nav2_behavior_tree
          {
          /**
           * @brief A BT::ConditionNode that returns SUCCESS when goal is
           * updated on the blackboard and FAILURE otherwise
           */
      33  class GloballyUpdatedGoalCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GloballyUpdatedGoalCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      41   GloballyUpdatedGoalCondition(  
      42   const std::string & condition_name,  
      43   const BT::NodeConfiguration & conf );
          
           GloballyUpdatedGoalCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {};
           }
          
          private:
           bool first_time;
           rclcpp::Node::SharedPtr node_;
           geometry_msgs::msg::PoseStamped goal_;
           std::vector<geometry_msgs::msg::PoseStamped> goals_;
          };
          
          } // namespace nav2_behavior_tree
          
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "tf2_ros/buffer.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS when a specified goal
           * is reached and FAILURE otherwise
           */
      32  class GoalReachedCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GoalReachedCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      40   GoalReachedCondition(  
      41   const std::string & condition_name,  
      42   const BT::NodeConfiguration & conf );
          
           GoalReachedCondition(   ) = delete;
          
           /**
           * @brief A destructor for nav2_behavior_tree::GoalReachedCondition
           */
           ~GoalReachedCondition(   ) override;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Function to read parameters and initialize class variables
           */
           void initialize(   );
          
           /**
           * @brief Checks if the current robot pose lies within a given distance from the goal
           * @return bool true when goal is reached,   false otherwise
           */
           bool isGoalReached(   );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  "goal",   "Destination" ),  
           BT::InputPort<std::string>(  "global_frame",   std::string(  "map" ),   "Global frame" ),  
           BT::InputPort<std::string>(  "robot_base_frame",   std::string(  "base_link" ),   "Robot base frame" )
           };
           }
          
          protected:
           /**
           * @brief Cleanup function
           */
      85   void cleanup(   )
           {}
          
          private:
           rclcpp::Node::SharedPtr node_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           bool initialized_;
           double goal_reached_tol_;
           std::string global_frame_;
           std::string robot_base_frame_;
           double transform_tolerance_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

          // Copyright (  c ) 2020 Aitor Miguel Blanco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
          
          #include <string>
          #include <vector>
          
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS when goal is
           * updated on the blackboard and FAILURE otherwise
           */
      31  class GoalUpdatedCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GoalUpdatedCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      39   GoalUpdatedCondition(  
      40   const std::string & condition_name,  
      41   const BT::NodeConfiguration & conf );
          
           GoalUpdatedCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {};
           }
          
          private:
           geometry_msgs::msg::PoseStamped goal_;
           std::vector<geometry_msgs::msg::PoseStamped> goals_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
          
          #include "behaviortree_cpp_v3/behavior_tree.h"
          
          namespace nav2_behavior_tree
          {
          /**
           * @brief A BT::ConditionNode that returns SUCCESS if initial pose
           * has been received and FAILURE otherwise
           */
      26  BT::NodeStatus initialPoseReceived(  BT::TreeNode & tree_node );
          }
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
          
          #include <string>
          #include <memory>
          #include <mutex>
          
          #include "rclcpp/rclcpp.hpp"
          #include "sensor_msgs/msg/battery_state.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that listens to a battery topic and
           * returns SUCCESS when battery is low and FAILURE otherwise
           */
      34  class IsBatteryLowCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::IsBatteryLowCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      42   IsBatteryLowCondition(  
      43   const std::string & condition_name,  
      44   const BT::NodeConfiguration & conf );
          
           IsBatteryLowCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "min_battery",   "Minimum battery percentage/voltage" ),  
           BT::InputPort<std::string>(  
           "battery_topic",   std::string(  "/battery_status" ),   "Battery topic" ),  
           BT::InputPort<bool>(  
           "is_voltage",   false,   "If true voltage will be used to check for low battery" ),  
           };
           }
          
          private:
           /**
           * @brief Callback function for battery topic
           * @param msg Shared pointer to sensor_msgs::msg::BatteryState message
           */
      74   void batteryCallback(  sensor_msgs::msg::BatteryState::SharedPtr msg );
          
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
           rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
           std::string battery_topic_;
           double min_battery_;
           bool is_voltage_;
           bool is_battery_low_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

          // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_msgs/srv/is_path_valid.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS when the IsPathValid
           * service returns true and FAILURE otherwise
           */
      33  class IsPathValidCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::IsPathValidCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      41   IsPathValidCondition(  
      42   const std::string & condition_name,  
      43   const BT::NodeConfiguration & conf );
          
           IsPathValidCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<nav_msgs::msg::Path>(  "path",   "Path to Check" ),  
           BT::InputPort<std::chrono::milliseconds>(  "server_timeout" )
           };
           }
          
          private:
           rclcpp::Node::SharedPtr node_;
           rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
           // The timeout value while waiting for a responce from the
           // is path valid service
           std::chrono::milliseconds server_timeout_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
          
          #include <string>
          #include <atomic>
          #include <deque>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "nav_msgs/msg/odometry.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS
           * if robot is stuck somewhere and FAILURE otherwise
           */
      33  class IsStuckCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::IsStuckCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      41   IsStuckCondition(  
      42   const std::string & condition_name,  
      43   const BT::NodeConfiguration & conf );
          
           IsStuckCondition(   ) = delete;
          
           /**
           * @brief A destructor for nav2_behavior_tree::IsStuckCondition
           */
           ~IsStuckCondition(   ) override;
          
           /**
           * @brief Callback function for odom topic
           * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message
           */
           void onOdomReceived(  const typename nav_msgs::msg::Odometry::SharedPtr msg );
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Function to log status when robot is stuck/free
           */
           void logStuck(  const std::string & msg ) const;
          
           /**
           * @brief Function to approximate acceleration from the odom history
           */
           void updateStates(   );
          
           /**
           * @brief Detect if robot bumped into something by checking for abnormal deceleration
           * @return bool true if robot is stuck,   false otherwise
           */
           bool isStuck(   );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   ) {return {};}
          
          private:
           // The node that will be used for any ROS operations
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
           std::thread callback_group_executor_thread;
          
           std::atomic<bool> is_stuck_;
          
           // Listen to odometry
           rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
           // Store history of odometry measurements
           std::deque<nav_msgs::msg::Odometry> odom_history_;
           std::deque<nav_msgs::msg::Odometry>::size_type odom_history_size_;
          
           // Calculated states
           double current_accel_;
          
           // Robot specific paramters
           double brake_accel_limit_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp

          // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
          
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "nav_msgs/msg/path.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS every time a specified
           * time period passes and FAILURE otherwise
           */
      31  class PathExpiringTimerCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::PathExpiringTimerCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      39   PathExpiringTimerCondition(  
      40   const std::string & condition_name,  
      41   const BT::NodeConfiguration & conf );
          
           PathExpiringTimerCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "seconds",   1.0,   "Seconds" ),  
           BT::InputPort<nav_msgs::msg::Path>(  "path" )
           };
           }
          
          private:
           rclcpp::Node::SharedPtr node_;
           rclcpp::Time start_;
           nav_msgs::msg::Path prev_path_;
           double period_;
           bool first_time_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp

          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
          
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS every time a specified
           * time period passes and FAILURE otherwise
           */
      31  class TimeExpiredCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::TimeExpiredCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      39   TimeExpiredCondition(  
      40   const std::string & condition_name,  
      41   const BT::NodeConfiguration & conf );
          
           TimeExpiredCondition(   ) = delete;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "seconds",   1.0,   "Seconds" )
           };
           }
          
          private:
           rclcpp::Node::SharedPtr node_;
           rclcpp::Time start_;
           double period_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp

          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
          
          #include <string>
          #include <atomic>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/condition_node.h"
          #include "tf2_ros/buffer.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform
           * between two specified frames and FAILURE otherwise
           */
      33  class TransformAvailableCondition : public BT::ConditionNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::TransformAvailableCondition
           * @param condition_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      41   TransformAvailableCondition(  
      42   const std::string & condition_name,  
      43   const BT::NodeConfiguration & conf );
          
           TransformAvailableCondition(   ) = delete;
          
           /**
           * @brief A destructor for nav2_behavior_tree::TransformAvailableCondition
           */
           ~TransformAvailableCondition(   );
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<std::string>(  "child",   std::string(   ),   "Child frame for transform" ),  
           BT::InputPort<std::string>(  "parent",   std::string(   ),   "parent frame for transform" )
           };
           }
          
          private:
           rclcpp::Node::SharedPtr node_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           std::atomic<bool> was_found_;
          
           std::string child_frame_;
           std::string parent_frame_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
          
          #include <string>
          #include "behaviortree_cpp_v3/control_node.h"
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          namespace nav2_behavior_tree
          {
          
          /** @brief Type of sequence node that re-ticks previous children when a child returns running
           *
           * Type of Control Node | Child Returns Failure | Child Returns Running
           * ---------------------------------------------------------------------
           * PipelineSequence | Restart | Tick All Previous Again
           *
           * Tick All Previous Again means every node up till this one will be reticked. Even
           * if a previous node returns Running,   the next node will be reticked.
           *
           * As an example,   let's say this node has 3 children: A,   B and C. At the start,  
           * they are all IDLE.
           * | A | B | C |
           * --------------------------------
           * | IDLE | IDLE | IDLE |
           * | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
           * - PipelineSequence returns RUNNING and no other nodes are ticked.
           * | SUCCESS | RUNNING | IDLE | - This time A returns SUCCESS so B gets ticked as well
           * - PipelineSequence returns RUNNING and C is not ticked yet
           * | RUNNING | SUCCESS | RUNNING | - A gets ticked and returns RUNNING,   but since it had previously
           * - returned SUCCESS,   PipelineSequence continues on and ticks B.
           * - Since B also returns SUCCESS,   C gets ticked this time as well.
           * | RUNNING | SUCCESS | SUCCESS | - A is still RUNNING,   and B returns SUCCESS again. This time C
           * - returned SUCCESS,   ending the sequence. PipelineSequence
           * - returns SUCCESS and halts A.
           *
           * If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
           * and halted all children,   ending the sequence.
           *
           * Usage in XML: <PipelineSequence>
           */
      55  class PipelineSequence : public BT::ControlNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::PipelineSequence
           * @param name Name for the XML tag for this node
           */
      62   explicit PipelineSequence(  const std::string & name );
          
           /**
           * @brief A constructor for nav2_behavior_tree::PipelineSequence
           * @param name Name for the XML tag for this node
           * @param config BT node configuration
           */
      69   PipelineSequence(  const std::string & name,   const BT::NodeConfiguration & config );
          
           /**
           * @brief The other (  optional ) override required by a BT action to reset node state
           */
           void halt(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   ) {return {};}
          
          protected:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
      87   BT::NodeStatus tick(   ) override;
          
           std::size_t last_child_ticked_ = 0;
          };
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
          
          #include <string>
          #include "behaviortree_cpp_v3/control_node.h"
          
          namespace nav2_behavior_tree
          {
          /**
           * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
           * returns SUCCESS.
           *
           * - If the first child returns FAILURE,   the second child will be executed. After that the first
           * child is executed again if the second child returns SUCCESS.
           *
           * - If the first or second child returns RUNNING,   this node returns RUNNING.
           *
           * - If the second child returns FAILURE,   this control node will stop the loop and returns FAILURE.
           *
           */
      35  class RecoveryNode : public BT::ControlNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::RecoveryNode
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      43   RecoveryNode(  
      44   const std::string & name,  
      45   const BT::NodeConfiguration & conf );
          
           /**
           * @brief A destructor for nav2_behavior_tree::RecoveryNode
           */
           ~RecoveryNode(   ) override = default;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<int>(  "number_of_retries",   1,   "Number of retries" )
           };
           }
          
          private:
           unsigned int current_child_idx_;
           unsigned int number_of_retries_;
           unsigned int retry_count_;
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
      72   BT::NodeStatus tick(   ) override;
          
           /**
           * @brief The other (  optional ) override required by a BT action to reset node state
           */
      77   void halt(   ) override;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
          
          #include <string>
          
          #include "behaviortree_cpp_v3/control_node.h"
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          namespace nav2_behavior_tree
          {
          
          /** @brief Type of sequence node that ticks children in a round-robin fashion
           *
           * Type of Control Node | Child Returns Failure | Child Returns Running
           * ---------------------------------------------------------------------
           * RoundRobin | Tick Next Child | Return Running
           *
           * If the current child return failure,   the next child is ticked and if the last child returns
           * failure,   the first child is ticked and the cycle continues until a child returns success
           *
           * As an example,   let's say this node has 3 children: A,   B and C. At the start,  
           * they are all IDLE.
           * | A | B | C |
           * --------------------------------
           * | IDLE | IDLE | IDLE |
           * | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
           * - RoundRobin returns RUNNING and no other nodes are ticked.
           * | FAILURE | RUNNING | IDLE | - A returns FAILURE so B gets ticked and returns RUNNING
           * - RoundRobin returns RUNNING and C is not ticked yet
           * | FAILURE | SUCCESS | IDLE | - B returns SUCCESS,   so RoundRobin halts all children and
           * - returns SUCCESS,   next iteration will tick C.
           * | RUNNING | IDLE | FAILURE | - C returns FAILURE,   so RoundRobin circles and ticks A.
           * - A returns RUNNING,   so RoundRobin returns RUNNING.
           *
           * If all children return FAILURE,   RoundRobin will return FAILURE
           * and halt all children,   ending the sequence.
           *
           * Usage in XML: <RoundRobin>
           */
      54  class RoundRobinNode : public BT::ControlNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::RoundRobinNode
           * @param name Name for the XML tag for this node
           */
      61   explicit RoundRobinNode(  const std::string & name );
          
           /**
           * @brief A constructor for nav2_behavior_tree::RoundRobinNode
           * @param name Name for the XML tag for this node
           * @param config BT node configuration
           */
      68   RoundRobinNode(  const std::string & name,   const BT::NodeConfiguration & config );
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief The other (  optional ) override required by a BT action to reset node state
           */
           void halt(   ) override;
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing basic ports along with node-specific ports
           */
           static BT::PortsList providedPorts(   ) {return {};}
          
          private:
           unsigned int current_child_idx_{0};
           unsigned int num_failed_children_{0};
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "tf2_ros/buffer.h"
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that ticks its child every time the robot
           * travels a specified distance
           */
      34  class DistanceController : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::DistanceController
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      42   DistanceController(  
      43   const std::string & name,  
      44   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      50   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "distance",   1.0,   "Distance" ),  
           BT::InputPort<std::string>(  "global_frame",   std::string(  "map" ),   "Global frame" ),  
           BT::InputPort<std::string>(  "robot_base_frame",   std::string(  "base_link" ),   "Robot base frame" )
           };
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           rclcpp::Node::SharedPtr node_;
          
           std::shared_ptr<tf2_ros::Buffer> tf_;
           double transform_tolerance_;
          
           geometry_msgs::msg::PoseStamped start_pose_;
           double distance_;
          
           std::string global_frame_;
           std::string robot_base_frame_;
          
           bool first_time_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
          
          #include <chrono>
          #include <string>
          #include <vector>
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that ticks its child if the goal was updated
           */
      33  class GoalUpdatedController : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GoalUpdatedController
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      41   GoalUpdatedController(  
      42   const std::string & name,  
      43   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      49   static BT::PortsList providedPorts(   )
           {
           return {};
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           bool goal_was_updated_;
           geometry_msgs::msg::PoseStamped goal_;
           std::vector<geometry_msgs::msg::PoseStamped> goals_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that subscribes to a goal topic and updates
           * the current goal on the blackboard
           */
      35  class GoalUpdater : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::GoalUpdater
           * @param xml_tag_name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      43   GoalUpdater(  
      44   const std::string & xml_tag_name,  
      45   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      51   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<geometry_msgs::msg::PoseStamped>(  "input_goal",   "Original Goal" ),  
           BT::OutputPort<geometry_msgs::msg::PoseStamped>(  
           "output_goal",  
           "Received Goal by subscription" ),  
           };
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Callback function for goal update topic
           * @param msg Shared pointer to geometry_msgs::msg::PoseStamped message
           */
           void callback_updated_goal(  const geometry_msgs::msg::PoseStamped::SharedPtr msg );
          
           rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
          
           geometry_msgs::msg::PoseStamped last_goal_received_;
          
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
          
          #include <string>
          #include <memory>
          #include <limits>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "behaviortree_cpp_v3/decorator_node.h"
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that ticks its child everytime when the length of
           * the new path is smaller than the old one by the length given by the user.
           */
      34  class PathLongerOnApproach : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::PathLongerOnApproach
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      42   PathLongerOnApproach(  
      43   const std::string & name,  
      44   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      50   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<nav_msgs::msg::Path>(  "path",   "Planned Path" ),  
           BT::InputPort<double>(  
           "prox_len",   3.0,  
           "Proximity length (  m ) for the path to be longer on approach" ),  
           BT::InputPort<double>(  
           "length_factor",   2.0,  
           "Length multiplication factor to check if the path is significantly longer" ),  
           };
           }
          
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
          private:
           /**
           * @brief Checks if the global path is updated
           * @param new_path new path to the goal
           * @param old_path current path to the goal
           * @return whether the path is updated for the current goal
           */
           bool isPathUpdated(  
           nav_msgs::msg::Path & new_path,  
           nav_msgs::msg::Path & old_path );
          
           /**
           * @brief Checks if the robot is in the goal proximity
           * @param old_path current path to the goal
           * @param prox_leng proximity length from the goal
           * @return whether the robot is in the goal proximity
           */
           bool isRobotInGoalProximity(  
           nav_msgs::msg::Path & old_path,  
           double & prox_leng );
          
           /**
           * @brief Checks if the new path is longer
           * @param new_path new path to the goal
           * @param old_path current path to the goal
           * @param length_factor multipler for path length check
           * @return whether the new path is longer
           */
           bool isNewPathLonger(  
           nav_msgs::msg::Path & new_path,  
           nav_msgs::msg::Path & old_path,  
           double & length_factor );
          
          private:
           nav_msgs::msg::Path new_path_;
           nav_msgs::msg::Path old_path_;
           double prox_len_ = std::numeric_limits<double>::max(   );
           double length_factor_ = std::numeric_limits<double>::max(   );
           rclcpp::Node::SharedPtr node_;
           bool first_time_ = true;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
          
          #include <chrono>
          #include <string>
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that ticks its child at a specified rate
           */
      29  class RateController : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::RateController
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      37   RateController(  
      38   const std::string & name,  
      39   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      45   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "hz",   10.0,   "Rate" )
           };
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           std::chrono::time_point<std::chrono::high_resolution_clock> start_;
           double period_;
           bool first_time_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
          
          #include <chrono>
          #include <string>
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE
           * for every succeeding tick
           */
      30  class SingleTrigger : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::SingleTrigger
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      38   SingleTrigger(  
      39   const std::string & name,  
      40   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      46   static BT::PortsList providedPorts(   )
           {
           return {};
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           bool first_time_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
          #define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <deque>
          
          #include "nav_msgs/msg/odometry.hpp"
          #include "nav2_util/odometry_utils.hpp"
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A BT::DecoratorNode that ticks its child every at a rate proportional to
           * the speed of the robot. If the robot travels faster,   this node will tick its child at a
           * higher frequency and reduce the tick frequency if the robot slows down
           */
      37  class SpeedController : public BT::DecoratorNode
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::SpeedController
           * @param name Name for the XML tag for this node
           * @param conf BT node configuration
           */
      45   SpeedController(  
      46   const std::string & name,  
      47   const BT::NodeConfiguration & conf );
          
           /**
           * @brief Creates list of BT ports
           * @return BT::PortsList Containing node-specific ports
           */
      53   static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<double>(  "min_rate",   0.1,   "Minimum rate" ),  
           BT::InputPort<double>(  "max_rate",   1.0,   "Maximum rate" ),  
           BT::InputPort<double>(  "min_speed",   0.0,   "Minimum speed" ),  
           BT::InputPort<double>(  "max_speed",   0.5,   "Maximum speed" ),  
           BT::InputPort<double>(  "filter_duration",   0.3,   "Duration (  secs ) for velocity smoothing filter" )
           };
           }
          
          private:
           /**
           * @brief The main override required by a BT action
           * @return BT::NodeStatus Status of tick execution
           */
           BT::NodeStatus tick(   ) override;
          
           /**
           * @brief Scale the rate based speed
           * @return double Rate scaled by speed limits and clamped
           */
           inline double getScaledRate(  const double & speed )
           {
           return std::max(  
           std::min(  
           (  (  (  speed - min_speed_ ) / d_speed_ ) * d_rate_ ) + min_rate_,  
           max_rate_ ),   min_rate_ );
           }
          
           /**
           * @brief Update period based on current smoothed speed and reset timer
           */
           inline void updatePeriod(   )
           {
           auto velocity = odom_smoother_->getTwist(   );
           double speed = std::hypot(  velocity.linear.x,   velocity.linear.y );
           double rate = getScaledRate(  speed );
      91   period_ = 1.0 / rate;
           }
          
           rclcpp::Node::SharedPtr node_;
          
           // To keep track of time to reset
           rclcpp::Time start_;
          
           // To get a smoothed velocity
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
          
           bool first_tick_;
          
           // Time period after which child node should be ticked
           double period_;
          
           // Rates thresholds to tick child node
           double min_rate_;
           double max_rate_;
           double d_rate_;
          
           // Speed thresholds
           double min_speed_;
           double max_speed_;
           double d_speed_;
          
           // current goal
           geometry_msgs::msg::PoseStamped goal_;
           std::vector<geometry_msgs::msg::PoseStamped> goals_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
          #define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
          
          #include <vector>
          #include <memory>
          #include <utility>
          
          #include "behaviortree_cpp_v3/loggers/abstract_logger.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_msgs/msg/behavior_tree_log.hpp"
          #include "nav2_msgs/msg/behavior_tree_status_change.h"
          #include "tf2_ros/buffer_interface.h"
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A class to publish BT logs on BT status change
           */
      34  class RosTopicLogger : public BT::StatusChangeLogger
          {
          public:
           /**
           * @brief A constructor for nav2_behavior_tree::RosTopicLogger
           * @param ros_node Weak pointer to parent rclcpp::Node
           * @param tree BT to monitor
           */
      42   RosTopicLogger(  const rclcpp::Node::WeakPtr & ros_node,   const BT::Tree & tree )
           : StatusChangeLogger(  tree.rootNode(   ) )
           {
           auto node = ros_node.lock(   );
           clock_ = node->get_clock(   );
           logger_ = node->get_logger(   );
           log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(  
           "behavior_tree_log",  
           rclcpp::QoS(  10 ) );
           }
          
           /**
           * @brief Callback function which is called each time BT changes status
           * @param timestamp Timestamp of BT status change
           * @param node Node that changed status
           * @param prev_status Previous status of the node
           * @param status Current status of the node
           */
           void callback(  
           BT::Duration timestamp,  
           const BT::TreeNode & node,  
           BT::NodeStatus prev_status,  
           BT::NodeStatus status ) override
           {
           nav2_msgs::msg::BehaviorTreeStatusChange event;
          
           // BT timestamps are a duration since the epoch. Need to convert to a time_point
           // before converting to a msg.
           event.timestamp = tf2_ros::toMsg(  tf2::TimePoint(  timestamp ) );
           event.node_name = node.name(   );
           event.previous_status = toStr(  prev_status,   false );
           event.current_status = toStr(  status,   false );
           event_log_.push_back(  std::move(  event ) );
          
           RCLCPP_DEBUG(  
           logger_,   "[%.3f]: %25s %s -> %s",  
           std::chrono::duration<double>(  timestamp ).count(   ),  
           node.name(   ).c_str(   ),  
           toStr(  prev_status,   true ).c_str(   ),  
           toStr(  status,   true ).c_str(   )  );
           }
          
           /**
           * @brief Clear log buffer if any
           */
           void flush(   ) override
           {
           if (  !event_log_.empty(   ) ) {
           auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>(   );
      91   log_msg->timestamp = clock_->now(   );
      92   log_msg->event_log = event_log_;
      93   log_pub_->publish(  std::move(  log_msg ) );
      94   event_log_.clear(   );
           }
           }
          
          protected:
           rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "bt_navigator" )};
           rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
           std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_

navigation2/nav2_behavior_tree/plugins/action/back_up_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  BackUpAction::BackUpAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::BackUp>(  xml_tag_name,   action_name,   conf )
          {
           double dist;
           getInput(  "backup_dist",   dist );
           double speed;
           getInput(  "backup_speed",   speed );
           double time_allowance;
           getInput(  "time_allowance",   time_allowance );
          
           // Populate the input message
           goal_.target.x = dist;
           goal_.target.y = 0.0;
           goal_.target.z = 0.0;
           goal_.speed = speed;
           goal_.time_allowance = rclcpp::Duration::from_seconds(  time_allowance );
          }
          
      44  void BackUpAction::on_tick(   )
          {
           increment_recovery_count(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      52  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::BackUpAction>(  
           name,   "backup",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::BackUpAction>(  "BackUp",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  BackUpCancel::BackUpCancel(  
      26   const std::string & xml_tag_name,  
      27   const std::string & action_name,  
      28   const BT::NodeConfiguration & conf )
          : BtCancelActionNode<nav2_msgs::action::BackUp>(  xml_tag_name,   action_name,   conf )
          {
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      36  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::BackUpCancel>(  
           name,   "backup",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::BackUpCancel>(  
           "CancelBackUp",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  ClearEntireCostmapService::ClearEntireCostmapService(  
      24   const std::string & service_node_name,  
      25   const BT::NodeConfiguration & conf )
          : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(  service_node_name,   conf )
          {
          }
          
      30  void ClearEntireCostmapService::on_tick(   )
          {
           increment_recovery_count(   );
          }
          
      35  ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(  
      36   const std::string & service_node_name,  
      37   const BT::NodeConfiguration & conf )
          : BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>(  service_node_name,   conf )
          {
          }
          
      42  void ClearCostmapExceptRegionService::on_tick(   )
          {
           getInput(  "reset_distance",   request_->reset_distance );
           increment_recovery_count(   );
          }
          
      48  ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(  
      49   const std::string & service_node_name,  
      50   const BT::NodeConfiguration & conf )
          : BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>(  service_node_name,   conf )
          {
          }
          
      55  void ClearCostmapAroundRobotService::on_tick(   )
          {
           getInput(  "reset_distance",   request_->reset_distance );
           increment_recovery_count(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      64  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>(  "ClearEntireCostmap" );
           factory.registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(  
           "ClearCostmapExceptRegion" );
           factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(  
           "ClearCostmapAroundRobot" );
          }

navigation2/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      24  ComputePathThroughPosesAction::ComputePathThroughPosesAction(  
      25   const std::string & xml_tag_name,  
      26   const std::string & action_name,  
      27   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::ComputePathThroughPoses>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      32  void ComputePathThroughPosesAction::on_tick(   )
          {
           getInput(  "goals",   goal_.goals );
           getInput(  "planner_id",   goal_.planner_id );
           if (  getInput(  "start",   goal_.start ) ) {
           goal_.use_start = true;
           }
          }
          
      41  BT::NodeStatus ComputePathThroughPosesAction::on_success(   )
          {
           setOutput(  "path",   result_.result->path );
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      50  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(  
           name,   "compute_path_through_poses",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(  
           "ComputePathThroughPoses",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  ComputePathToPoseAction::ComputePathToPoseAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::ComputePathToPose>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      31  void ComputePathToPoseAction::on_tick(   )
          {
           getInput(  "goal",   goal_.goal );
           getInput(  "planner_id",   goal_.planner_id );
           if (  getInput(  "start",   goal_.start ) ) {
           goal_.use_start = true;
           }
          }
          
      40  BT::NodeStatus ComputePathToPoseAction::on_success(   )
          {
           setOutput(  "path",   result_.result->path );
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      49  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(  
           name,   "compute_path_to_pose",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(  
           "ComputePathToPose",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  ControllerCancel::ControllerCancel(  
      26   const std::string & xml_tag_name,  
      27   const std::string & action_name,  
      28   const BT::NodeConfiguration & conf )
          : BtCancelActionNode<nav2_msgs::action::FollowPath>(  xml_tag_name,   action_name,   conf )
          {
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      36  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ControllerCancel>(  
           name,   "follow_path",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::ControllerCancel>(  
           "CancelControl",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/controller_selector_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          using std::placeholders::_1;
          
      30  ControllerSelector::ControllerSelector(  
      31   const std::string & name,  
      32   const BT::NodeConfiguration & conf )
          : BT::SyncActionNode(  name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           getInput(  "topic_name",   topic_name_ );
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           rclcpp::SubscriptionOptions sub_option;
           sub_option.callback_group = callback_group_;
           controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(  
           topic_name_,  
           qos,  
           std::bind(  &ControllerSelector::callbackControllerSelect,   this,   _1 ),  
           sub_option );
          }
          
      55  BT::NodeStatus ControllerSelector::tick(   )
          {
           callback_group_executor_.spin_some(   );
          
           // This behavior always use the last selected controller received from the topic input.
           // When no input is specified it uses the default controller.
           // If the default controller is not specified then we work in "required controller mode":
           // In this mode,   the behavior returns failure if the controller selection is not received from
           // the topic input.
           if (  last_selected_controller_.empty(   ) ) {
           std::string default_controller;
           getInput(  "default_controller",   default_controller );
           if (  default_controller.empty(   ) ) {
           return BT::NodeStatus::FAILURE;
           } else {
           last_selected_controller_ = default_controller;
           }
           }
          
           setOutput(  "selected_controller",   last_selected_controller_ );
          
           return BT::NodeStatus::SUCCESS;
          }
          
          void
      80  ControllerSelector::callbackControllerSelect(  const std_msgs::msg::String::SharedPtr msg )
          {
           last_selected_controller_ = msg->data;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      88  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::ControllerSelector>(  "ControllerSelector" );
          }

navigation2/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  DriveOnHeadingAction::DriveOnHeadingAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::DriveOnHeading>(  xml_tag_name,   action_name,   conf )
          {
           double dist;
           getInput(  "dist_to_travel",   dist );
           double speed;
           getInput(  "speed",   speed );
           double time_allowance;
           getInput(  "time_allowance",   time_allowance );
          
           // Populate the input message
           goal_.target.x = dist;
           goal_.target.y = 0.0;
           goal_.target.z = 0.0;
           goal_.speed = speed;
           goal_.time_allowance = rclcpp::Duration::from_seconds(  time_allowance );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      47  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(  
           name,   "drive_on_heading",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>(  "DriveOnHeading",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  DriveOnHeadingCancel::DriveOnHeadingCancel(  
      26   const std::string & xml_tag_name,  
      27   const std::string & action_name,  
      28   const BT::NodeConfiguration & conf )
          : BtCancelActionNode<nav2_msgs::action::DriveOnHeading>(  xml_tag_name,   action_name,   conf )
          {
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      36  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(  
           name,   "drive_on_heading",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(  
           "CancelDriveOnHeading",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/follow_path_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  FollowPathAction::FollowPathAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::FollowPath>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      31  void FollowPathAction::on_tick(   )
          {
           getInput(  "path",   goal_.path );
           getInput(  "controller_id",   goal_.controller_id );
           getInput(  "goal_checker_id",   goal_.goal_checker_id );
          }
          
      38  void FollowPathAction::on_wait_for_result(  
      39   std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/ )
          {
           // Grab the new path
           nav_msgs::msg::Path new_path;
           getInput(  "path",   new_path );
          
           // Check if it is not same with the current one
           if (  goal_.path != new_path ) {
           // the action server on the next loop iteration
           goal_.path = new_path;
           goal_updated_ = true;
           }
          
           std::string new_controller_id;
           getInput(  "controller_id",   new_controller_id );
          
           if (  goal_.controller_id != new_controller_id ) {
           goal_.controller_id = new_controller_id;
           goal_updated_ = true;
           }
          
           std::string new_goal_checker_id;
           getInput(  "goal_checker_id",   new_goal_checker_id );
          
           if (  goal_.goal_checker_id != new_goal_checker_id ) {
           goal_.goal_checker_id = new_goal_checker_id;
           goal_updated_ = true;
           }
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      72  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::FollowPathAction>(  
           name,   "follow_path",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(  
           "FollowPath",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          using std::placeholders::_1;
          
      30  GoalCheckerSelector::GoalCheckerSelector(  
      31   const std::string & name,  
      32   const BT::NodeConfiguration & conf )
          : BT::SyncActionNode(  name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          
           getInput(  "topic_name",   topic_name_ );
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(  
           topic_name_,   qos,   std::bind(  &GoalCheckerSelector::callbackGoalCheckerSelect,   this,   _1 ) );
          }
          
      46  BT::NodeStatus GoalCheckerSelector::tick(   )
          {
           rclcpp::spin_some(  node_ );
          
           // This behavior always use the last selected goal checker received from the topic input.
           // When no input is specified it uses the default goal checker.
           // If the default goal checker is not specified then we work in "required goal checker mode":
           // In this mode,   the behavior returns failure if the goal checker selection is not received from
           // the topic input.
           if (  last_selected_goal_checker_.empty(   ) ) {
           std::string default_goal_checker;
           getInput(  "default_goal_checker",   default_goal_checker );
           if (  default_goal_checker.empty(   ) ) {
           return BT::NodeStatus::FAILURE;
           } else {
           last_selected_goal_checker_ = default_goal_checker;
           }
           }
          
           setOutput(  "selected_goal_checker",   last_selected_goal_checker_ );
          
           return BT::NodeStatus::SUCCESS;
          }
          
          void
      71  GoalCheckerSelector::callbackGoalCheckerSelect(  const std_msgs::msg::String::SharedPtr msg )
          {
           last_selected_goal_checker_ = msg->data;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      79  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GoalCheckerSelector>(  "GoalCheckerSelector" );
          }

navigation2/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  NavigateThroughPosesAction::NavigateThroughPosesAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::NavigateThroughPoses>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      31  void NavigateThroughPosesAction::on_tick(   )
          {
           if (  !getInput(  "goals",   goal_.poses ) ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "NavigateThroughPosesAction: goal not provided" );
           return;
           }
           getInput(  "behavior_tree",   goal_.behavior_tree );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      45  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(  
           name,   "navigate_through_poses",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(  
           "NavigateThroughPoses",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  NavigateToPoseAction::NavigateToPoseAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::NavigateToPose>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      31  void NavigateToPoseAction::on_tick(   )
          {
           if (  !getInput(  "goal",   goal_.pose ) ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "NavigateToPoseAction: goal not provided" );
           return;
           }
           getInput(  "behavior_tree",   goal_.behavior_tree );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      45  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(  
           name,   "navigate_to_pose",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(  
           "NavigateToPose",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/planner_selector_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          using std::placeholders::_1;
          
      30  PlannerSelector::PlannerSelector(  
      31   const std::string & name,  
      32   const BT::NodeConfiguration & conf )
          : BT::SyncActionNode(  name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           getInput(  "topic_name",   topic_name_ );
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           rclcpp::SubscriptionOptions sub_option;
           sub_option.callback_group = callback_group_;
           planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(  
           topic_name_,  
           qos,  
           std::bind(  &PlannerSelector::callbackPlannerSelect,   this,   _1 ),  
           sub_option );
          }
          
      55  BT::NodeStatus PlannerSelector::tick(   )
          {
           callback_group_executor_.spin_some(   );
          
           // This behavior always use the last selected planner received from the topic input.
           // When no input is specified it uses the default planner.
           // If the default planner is not specified then we work in "required planner mode":
           // In this mode,   the behavior returns failure if the planner selection is not received from
           // the topic input.
           if (  last_selected_planner_.empty(   ) ) {
           std::string default_planner;
           getInput(  "default_planner",   default_planner );
           if (  default_planner.empty(   ) ) {
           return BT::NodeStatus::FAILURE;
           } else {
           last_selected_planner_ = default_planner;
           }
           }
          
           setOutput(  "selected_planner",   last_selected_planner_ );
          
           return BT::NodeStatus::SUCCESS;
          }
          
          void
      80  PlannerSelector::callbackPlannerSelect(  const std_msgs::msg::String::SharedPtr msg )
          {
           last_selected_planner_ = msg->data;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      88  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::PlannerSelector>(  "PlannerSelector" );
          }

navigation2/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
          
          namespace nav2_behavior_tree
          {
          
      21  ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(  
      22   const std::string & service_node_name,  
      23   const BT::NodeConfiguration & conf )
          : BtServiceNode<std_srvs::srv::Empty>(  service_node_name,   conf )
          {}
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      30  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(  
           "ReinitializeGlobalLocalization" );
          }

navigation2/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          #include <limits>
          
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      27  RemovePassedGoals::RemovePassedGoals(  
      28   const std::string & name,  
      29   const BT::NodeConfiguration & conf )
          : BT::ActionNodeBase(  name,   conf ),  
           viapoint_achieved_radius_(  0.5 )
          {
           getInput(  "radius",   viapoint_achieved_radius_ );
          
           getInput(  "global_frame",   global_frame_ );
           getInput(  "robot_base_frame",   robot_base_frame_ );
           tf_ = config(   ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer" );
           auto node = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           node->get_parameter(  "transform_tolerance",   transform_tolerance_ );
          }
          
      42  inline BT::NodeStatus RemovePassedGoals::tick(   )
          {
           setStatus(  BT::NodeStatus::RUNNING );
          
           Goals goal_poses;
           getInput(  "input_goals",   goal_poses );
          
           if (  goal_poses.empty(   ) ) {
           setOutput(  "output_goals",   goal_poses );
           return BT::NodeStatus::SUCCESS;
           }
          
           using namespace nav2_util::geometry_utils; // NOLINT
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           return BT::NodeStatus::FAILURE;
           }
          
           double dist_to_goal;
           while (  goal_poses.size(   ) > 1 ) {
           dist_to_goal = euclidean_distance(  goal_poses[0].pose,   current_pose.pose );
          
           if (  dist_to_goal > viapoint_achieved_radius_ ) {
           break;
           }
          
           goal_poses.erase(  goal_poses.begin(   ) );
           }
          
           setOutput(  "output_goals",   goal_poses );
          
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      83  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::RemovePassedGoals>(  "RemovePassedGoals" );
          }

navigation2/nav2_behavior_tree/plugins/action/smooth_path_action.cpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      24  SmoothPathAction::SmoothPathAction(  
      25   const std::string & xml_tag_name,  
      26   const std::string & action_name,  
      27   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::SmoothPath>(  xml_tag_name,   action_name,   conf )
          {
          }
          
      32  void SmoothPathAction::on_tick(   )
          {
           getInput(  "unsmoothed_path",   goal_.path );
           getInput(  "smoother_id",   goal_.smoother_id );
           double max_smoothing_duration;
           getInput(  "max_smoothing_duration",   max_smoothing_duration );
           goal_.max_smoothing_duration = rclcpp::Duration::from_seconds(  max_smoothing_duration );
           getInput(  "check_for_collisions",   goal_.check_for_collisions );
          }
          
      42  BT::NodeStatus SmoothPathAction::on_success(   )
          {
           setOutput(  "smoothed_path",   result_.result->path );
           setOutput(  "smoothing_duration",   rclcpp::Duration(  result_.result->smoothing_duration ).seconds(   ) );
           setOutput(  "was_completed",   result_.result->was_completed );
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      53  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SmoothPathAction>(  
           name,   "smooth_path",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::SmoothPathAction>(  
           "SmoothPath",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/spin_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/action/spin_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  SpinAction::SpinAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::Spin>(  xml_tag_name,   action_name,   conf )
          {
           double dist;
           getInput(  "spin_dist",   dist );
           double time_allowance;
           getInput(  "time_allowance",   time_allowance );
           goal_.target_yaw = dist;
           goal_.time_allowance = rclcpp::Duration::from_seconds(  time_allowance );
           getInput(  "is_recovery",   is_recovery_ );
          }
          
      38  void SpinAction::on_tick(   )
          {
           if (  is_recovery_ ) {
           increment_recovery_count(   );
           }
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      48  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SpinAction>(  name,   "spin",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::SpinAction>(  "Spin",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  SpinCancel::SpinCancel(  
      26   const std::string & xml_tag_name,  
      27   const std::string & action_name,  
      28   const BT::NodeConfiguration & conf )
          : BtCancelActionNode<nav2_msgs::action::Spin>(  xml_tag_name,   action_name,   conf )
          {
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      36  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SpinCancel>(  
           name,   "spin",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::SpinCancel>(  
           "CancelSpin",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/truncate_path_action.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          #include <limits>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      30  TruncatePath::TruncatePath(  
      31   const std::string & name,  
      32   const BT::NodeConfiguration & conf )
          : BT::ActionNodeBase(  name,   conf ),  
           distance_(  1.0 )
          {
           getInput(  "distance",   distance_ );
          }
          
      39  inline BT::NodeStatus TruncatePath::tick(   )
          {
           setStatus(  BT::NodeStatus::RUNNING );
          
           nav_msgs::msg::Path input_path;
          
           getInput(  "input_path",   input_path );
          
           if (  input_path.poses.empty(   ) ) {
           setOutput(  "output_path",   input_path );
           return BT::NodeStatus::SUCCESS;
           }
          
           geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back(   );
          
           double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(  
           input_path.poses.back(   ),   final_pose );
          
           while (  distance_to_goal < distance_ && input_path.poses.size(   ) > 2 ) {
           input_path.poses.pop_back(   );
           distance_to_goal = nav2_util::geometry_utils::euclidean_distance(  
           input_path.poses.back(   ),   final_pose );
           }
          
           double dx = final_pose.pose.position.x - input_path.poses.back(   ).pose.position.x;
           double dy = final_pose.pose.position.y - input_path.poses.back(   ).pose.position.y;
          
           double final_angle = atan2(  dy,   dx );
          
           if (  std::isnan(  final_angle ) || std::isinf(  final_angle ) ) {
           RCLCPP_WARN(  
           config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" )->get_logger(   ),  
           "Final angle is not valid while truncating path. Setting to 0.0" );
           final_angle = 0.0;
           }
          
           input_path.poses.back(   ).pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  
           final_angle );
          
           setOutput(  "output_path",   input_path );
          
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      86  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::TruncatePath>(  "TruncatePath" );
          }

navigation2/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <limits>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "tf2_ros/create_timer_ros.h"
          
          #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      32  TruncatePathLocal::TruncatePathLocal(  
      33   const std::string & name,  
      34   const BT::NodeConfiguration & conf )
          : BT::ActionNodeBase(  name,   conf )
          {
           tf_buffer_ =
           config(   ).blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(  
           "tf_buffer" );
          }
          
      42  inline BT::NodeStatus TruncatePathLocal::tick(   )
          {
           setStatus(  BT::NodeStatus::RUNNING );
          
           double distance_forward,   distance_backward;
           geometry_msgs::msg::PoseStamped pose;
           double angular_distance_weight;
           double max_robot_pose_search_dist;
          
           getInput(  "distance_forward",   distance_forward );
           getInput(  "distance_backward",   distance_backward );
           getInput(  "angular_distance_weight",   angular_distance_weight );
           getInput(  "max_robot_pose_search_dist",   max_robot_pose_search_dist );
          
           bool path_pruning = std::isfinite(  max_robot_pose_search_dist );
           nav_msgs::msg::Path new_path;
           getInput(  "input_path",   new_path );
           if (  !path_pruning || new_path != path_ ) {
           path_ = new_path;
           closest_pose_detection_begin_ = path_.poses.begin(   );
           }
          
           if (  !getRobotPose(  path_.header.frame_id,   pose ) ) {
           return BT::NodeStatus::FAILURE;
           }
          
           if (  path_.poses.empty(   ) ) {
           setOutput(  "output_path",   path_ );
           return BT::NodeStatus::SUCCESS;
           }
          
           auto closest_pose_detection_end = path_.poses.end(   );
           if (  path_pruning ) {
           closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance(  
           closest_pose_detection_begin_,   path_.poses.end(   ),   max_robot_pose_search_dist );
           }
          
           // find the closest pose on the path
           auto current_pose = nav2_util::geometry_utils::min_by(  
           closest_pose_detection_begin_,   closest_pose_detection_end,  
           [&pose,   angular_distance_weight](  const geometry_msgs::msg::PoseStamped & ps ) {
           return poseDistance(  pose,   ps,   angular_distance_weight );
           } );
          
           if (  path_pruning ) {
           closest_pose_detection_begin_ = current_pose;
           }
          
           // expand forwards to extract desired length
           auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(  
           current_pose,   path_.poses.end(   ),   distance_forward );
          
           // expand backwards to extract desired length
           // Note: current_pose + 1 is used because reverse iterator points to a cell before it
           auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(  
           std::reverse_iterator(  current_pose + 1 ),   path_.poses.rend(   ),   distance_backward );
          
           nav_msgs::msg::Path output_path;
           output_path.header = path_.header;
           output_path.poses = std::vector<geometry_msgs::msg::PoseStamped>(  
           backward_pose_it.base(   ),   forward_pose_it );
           setOutput(  "output_path",   output_path );
          
           return BT::NodeStatus::SUCCESS;
          }
          
     108  inline bool TruncatePathLocal::getRobotPose(  
     109   std::string path_frame_id,   geometry_msgs::msg::PoseStamped & pose )
          {
           if (  !getInput(  "pose",   pose ) ) {
           std::string robot_frame;
           if (  !getInput(  "robot_frame",   robot_frame ) ) {
           RCLCPP_ERROR(  
           config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" )->get_logger(   ),  
           "Neither pose nor robot_frame specified for %s",   name(   ).c_str(   ) );
           return false;
           }
           double transform_tolerance;
           getInput(  "transform_tolerance",   transform_tolerance );
           if (  !nav2_util::getCurrentPose(  
           pose,   *tf_buffer_,   path_frame_id,   robot_frame,   transform_tolerance ) )
           {
           RCLCPP_WARN(  
           config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" )->get_logger(   ),  
           "Failed to lookup current robot pose for %s",   name(   ).c_str(   ) );
           return false;
           }
           }
           return true;
          }
          
          double
     134  TruncatePathLocal::poseDistance(  
     135   const geometry_msgs::msg::PoseStamped & pose1,  
     136   const geometry_msgs::msg::PoseStamped & pose2,  
           const double angular_distance_weight )
          {
           double dx = pose1.pose.position.x - pose2.pose.position.x;
           double dy = pose1.pose.position.y - pose2.pose.position.y;
           // taking angular distance into account in addition to spatial distance
           // (  to improve picking a correct pose near cusps and loops )
           tf2::Quaternion q1;
           tf2::convert(  pose1.pose.orientation,   q1 );
           tf2::Quaternion q2;
           tf2::convert(  pose2.pose.orientation,   q2 );
           double da = angular_distance_weight * std::abs(  q1.angleShortestPath(  q2 ) );
           return std::sqrt(  dx * dx + dy * dy + da * da );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     154  BT_REGISTER_NODES(  factory ) {
           factory.registerNodeType<nav2_behavior_tree::TruncatePathLocal>(  
           "TruncatePathLocal" );
          }

navigation2/nav2_behavior_tree/plugins/action/wait_action.cpp

       1  // Copyright (  c ) 2018 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/action/wait_action.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  WaitAction::WaitAction(  
      24   const std::string & xml_tag_name,  
      25   const std::string & action_name,  
      26   const BT::NodeConfiguration & conf )
          : BtActionNode<nav2_msgs::action::Wait>(  xml_tag_name,   action_name,   conf )
          {
           int duration;
           getInput(  "wait_duration",   duration );
           if (  duration <= 0 ) {
           RCLCPP_WARN(  
           node_->get_logger(   ),   "Wait duration is negative or zero "
           "(  %i ). Setting to positive.",   duration );
           duration *= -1;
           }
          
           goal_.time.sec = duration;
          }
          
      41  void WaitAction::on_tick(   )
          {
           increment_recovery_count(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      49  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::WaitAction>(  name,   "wait",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::WaitAction>(  "Wait",   builder );
          }

navigation2/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "std_msgs/msg/string.hpp"
          
          #include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  WaitCancel::WaitCancel(  
      26   const std::string & xml_tag_name,  
      27   const std::string & action_name,  
      28   const BT::NodeConfiguration & conf )
          : BtCancelActionNode<nav2_msgs::action::Wait>(  xml_tag_name,   action_name,   conf )
          {
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      36  BT_REGISTER_NODES(  factory )
          {
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::WaitCancel>(  
           name,   "wait",   config );
           };
          
           factory.registerBuilder<nav2_behavior_tree::WaitCancel>(  
           "CancelWait",   builder );
          }

navigation2/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      27  DistanceTraveledCondition::DistanceTraveledCondition(  
      28   const std::string & condition_name,  
      29   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           distance_(  1.0 ),  
           transform_tolerance_(  0.1 ),  
           global_frame_(  "map" ),  
           robot_base_frame_(  "base_link" )
          {
           getInput(  "distance",   distance_ );
           getInput(  "global_frame",   global_frame_ );
           getInput(  "robot_base_frame",   robot_base_frame_ );
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           tf_ = config(   ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer" );
           node_->get_parameter(  "transform_tolerance",   transform_tolerance_ );
          }
          
      44  BT::NodeStatus DistanceTraveledCondition::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           if (  !nav2_util::getCurrentPose(  
           start_pose_,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           }
           return BT::NodeStatus::FAILURE;
           }
          
           // Determine distance travelled since we've started this iteration
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           return BT::NodeStatus::FAILURE;
           }
          
           // Get euclidean distance
           auto travelled = nav2_util::geometry_utils::euclidean_distance(  
           start_pose_.pose,   current_pose.pose );
          
           if (  travelled < distance_ ) {
           return BT::NodeStatus::FAILURE;
           }
          
           // Update start pose
           start_pose_ = current_pose;
          
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      83  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>(  "DistanceTraveled" );
          }

navigation2/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

       1  // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(  
      24   const std::string & condition_name,  
      25   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           first_time(  true )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          }
          
      32  BT::NodeStatus GloballyUpdatedGoalCondition::tick(   )
          {
           if (  first_time ) {
           first_time = false;
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   goals_ );
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   goal_ );
           return BT::NodeStatus::FAILURE;
           }
          
           std::vector<geometry_msgs::msg::PoseStamped> current_goals;
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   current_goals );
           geometry_msgs::msg::PoseStamped current_goal;
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   current_goal );
          
           if (  goal_ != current_goal || goals_ != current_goals ) {
           goal_ = current_goal;
           goals_ = current_goals;
           return BT::NodeStatus::SUCCESS;
           }
          
           return BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      58  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>(  "GlobalUpdatedGoal" );
          }

navigation2/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_util/robot_utils.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/node_utils.hpp"
          
          #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      27  GoalReachedCondition::GoalReachedCondition(  
      28   const std::string & condition_name,  
      29   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           initialized_(  false ),  
           global_frame_(  "map" ),  
           robot_base_frame_(  "base_link" )
          {
           getInput(  "global_frame",   global_frame_ );
           getInput(  "robot_base_frame",   robot_base_frame_ );
          }
          
      39  GoalReachedCondition::~GoalReachedCondition(   )
          {
           cleanup(   );
          }
          
      44  BT::NodeStatus GoalReachedCondition::tick(   )
          {
           if (  !initialized_ ) {
           initialize(   );
           }
          
           if (  isGoalReached(   ) ) {
           return BT::NodeStatus::SUCCESS;
           }
           return BT::NodeStatus::FAILURE;
          }
          
      56  void GoalReachedCondition::initialize(   )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          
           nav2_util::declare_parameter_if_not_declared(  
           node_,   "goal_reached_tol",  
           rclcpp::ParameterValue(  0.25 ) );
           node_->get_parameter_or<double>(  "goal_reached_tol",   goal_reached_tol_,   0.25 );
           tf_ = config(   ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer" );
          
           node_->get_parameter(  "transform_tolerance",   transform_tolerance_ );
          
           initialized_ = true;
          }
          
      71  bool GoalReachedCondition::isGoalReached(   )
          {
           geometry_msgs::msg::PoseStamped current_pose;
          
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,   transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
          
           geometry_msgs::msg::PoseStamped goal;
           getInput(  "goal",   goal );
           double dx = goal.pose.position.x - current_pose.pose.position.x;
           double dy = goal.pose.position.y - current_pose.pose.position.y;
          
           return (  dx * dx + dy * dy ) <= (  goal_reached_tol_ * goal_reached_tol_ );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      93  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>(  "GoalReached" );
          }

navigation2/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

       1  // Copyright (  c ) 2020 Aitor Miguel Blanco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <vector>
          #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      22  GoalUpdatedCondition::GoalUpdatedCondition(  
      23   const std::string & condition_name,  
      24   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf )
          {}
          
      28  BT::NodeStatus GoalUpdatedCondition::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   goals_ );
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   goal_ );
           return BT::NodeStatus::FAILURE;
           }
          
           std::vector<geometry_msgs::msg::PoseStamped> current_goals;
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   current_goals );
           geometry_msgs::msg::PoseStamped current_goal;
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   current_goal );
          
           if (  goal_ != current_goal || goals_ != current_goals ) {
           goal_ = current_goal;
           goals_ = current_goals;
           return BT::NodeStatus::SUCCESS;
           }
          
           return BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      53  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>(  "GoalUpdated" );
          }

navigation2/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      20  BT::NodeStatus initialPoseReceived(  BT::TreeNode & tree_node )
          {
           auto initPoseReceived = tree_node.config(   ).blackboard->get<bool>(  "initial_pose_received" );
           return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      29  BT_REGISTER_NODES(  factory )
          {
           factory.registerSimpleCondition(  
           "InitialPoseReceived",  
           std::bind(  &nav2_behavior_tree::initialPoseReceived,   std::placeholders::_1 ) );
          }

navigation2/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  IsBatteryLowCondition::IsBatteryLowCondition(  
      24   const std::string & condition_name,  
      25   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           battery_topic_(  "/battery_status" ),  
           min_battery_(  0.0 ),  
           is_voltage_(  false ),  
           is_battery_low_(  false )
          {
           getInput(  "min_battery",   min_battery_ );
           getInput(  "battery_topic",   battery_topic_ );
           getInput(  "is_voltage",   is_voltage_ );
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           rclcpp::SubscriptionOptions sub_option;
           sub_option.callback_group = callback_group_;
           battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(  
           battery_topic_,  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &IsBatteryLowCondition::batteryCallback,   this,   std::placeholders::_1 ),  
           sub_option );
          }
          
      50  BT::NodeStatus IsBatteryLowCondition::tick(   )
          {
           callback_group_executor_.spin_some(   );
           if (  is_battery_low_ ) {
           return BT::NodeStatus::SUCCESS;
           }
           return BT::NodeStatus::FAILURE;
          }
          
      59  void IsBatteryLowCondition::batteryCallback(  sensor_msgs::msg::BatteryState::SharedPtr msg )
          {
           if (  is_voltage_ ) {
           is_battery_low_ = msg->voltage <= min_battery_;
           } else {
           is_battery_low_ = msg->percentage <= min_battery_;
           }
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      71  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>(  "IsBatteryLow" );
          }

navigation2/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

       1  // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
          #include <chrono>
          #include <memory>
          #include <string>
          
          namespace nav2_behavior_tree
          {
          
      23  IsPathValidCondition::IsPathValidCondition(  
      24   const std::string & condition_name,  
      25   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           client_ = node_->create_client<nav2_msgs::srv::IsPathValid>(  "is_path_valid" );
          
           server_timeout_ = config(   ).blackboard->template get<std::chrono::milliseconds>(  "server_timeout" );
           getInput<std::chrono::milliseconds>(  "server_timeout",   server_timeout_ );
          }
          
      35  BT::NodeStatus IsPathValidCondition::tick(   )
          {
           nav_msgs::msg::Path path;
           getInput(  "path",   path );
          
           auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>(   );
          
           request->path = path;
           auto result = client_->async_send_request(  request );
          
           if (  rclcpp::spin_until_future_complete(  node_,   result,   server_timeout_ ) ==
           rclcpp::FutureReturnCode::SUCCESS )
           {
           if (  result.get(   )->is_valid ) {
           return BT::NodeStatus::SUCCESS;
           }
           }
           return BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      58  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::IsPathValidCondition>(  "IsPathValid" );
          }

navigation2/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <chrono>
          
          #include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          
          namespace nav2_behavior_tree
          {
          
      25  IsStuckCondition::IsStuckCondition(  
      26   const std::string & condition_name,  
      27   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           is_stuck_(  false ),  
           odom_history_size_(  10 ),  
           current_accel_(  0.0 ),  
           brake_accel_limit_(  -10.0 )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
           callback_group_executor_thread = std::thread(  [this](   ) {callback_group_executor_.spin(   );} );
          
           rclcpp::SubscriptionOptions sub_option;
           sub_option.callback_group = callback_group_;
           odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(  
           "odom",  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &IsStuckCondition::onOdomReceived,   this,   std::placeholders::_1 ),  
           sub_option );
          
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Initialized an IsStuckCondition BT node" );
          
           RCLCPP_INFO_ONCE(  node_->get_logger(   ),   "Waiting on odometry" );
          }
          
      54  IsStuckCondition::~IsStuckCondition(   )
          {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Shutting down IsStuckCondition BT node" );
           callback_group_executor_.cancel(   );
           callback_group_executor_thread.join(   );
          }
          
      61  void IsStuckCondition::onOdomReceived(  const typename nav_msgs::msg::Odometry::SharedPtr msg )
          {
           RCLCPP_INFO_ONCE(  node_->get_logger(   ),   "Got odometry" );
          
           while (  odom_history_.size(   ) >= odom_history_size_ ) {
           odom_history_.pop_front(   );
           }
          
           odom_history_.push_back(  *msg );
          
           // TODO(  orduno ) #383 Move the state calculation and is stuck to robot class
           updateStates(   );
          }
          
      75  BT::NodeStatus IsStuckCondition::tick(   )
          {
           // TODO(  orduno ) #383 Once check for is stuck and state calculations are moved to robot class
           // this becomes
           // if (  robot_state_.isStuck(   ) ) {
          
           if (  is_stuck_ ) {
           logStuck(  "Robot got stuck!" );
           return BT::NodeStatus::SUCCESS; // Successfully detected a stuck condition
           }
          
           logStuck(  "Robot is free" );
           return BT::NodeStatus::FAILURE; // Failed to detected a stuck condition
          }
          
      90  void IsStuckCondition::logStuck(  const std::string & msg ) const
          {
           static std::string prev_msg;
          
           if (  msg == prev_msg ) {
           return;
           }
          
           RCLCPP_INFO(  node_->get_logger(   ),   msg.c_str(   ) );
           prev_msg = msg;
          }
          
     102  void IsStuckCondition::updateStates(   )
          {
           // Approximate acceleration
           // TODO(  orduno ) #400 Smooth out velocity history for better accel approx.
           if (  odom_history_.size(   ) > 2 ) {
           auto curr_odom = odom_history_.end(   )[-1];
           double curr_time = static_cast<double>(  curr_odom.header.stamp.sec );
           curr_time += (  static_cast<double>(  curr_odom.header.stamp.nanosec ) ) * 1e-9;
          
           auto prev_odom = odom_history_.end(   )[-2];
           double prev_time = static_cast<double>(  prev_odom.header.stamp.sec );
           prev_time += (  static_cast<double>(  prev_odom.header.stamp.nanosec ) ) * 1e-9;
          
           double dt = curr_time - prev_time;
           double vel_diff = static_cast<double>(  
           curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x );
           current_accel_ = vel_diff / dt;
           }
          
           is_stuck_ = isStuck(   );
          }
          
     124  bool IsStuckCondition::isStuck(   )
          {
           // TODO(  orduno ) #400 The robot getting stuck can result on different types of motion
           // depending on the state prior to getting stuck (  sudden change in accel,   not moving at all,  
           // random oscillations,   etc ). For now,   we only address the case where there is a sudden
           // harsh deceleration. A better approach to capture all situations would be to do a forward
           // simulation of the robot motion and compare it with the actual one.
          
           // Detect if robot bumped into something by checking for abnormal deceleration
           if (  current_accel_ < brake_accel_limit_ ) {
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Current deceleration is beyond brake limit."
           " brake limit: %.2f,   current accel: %.2f",   brake_accel_limit_,   current_accel_ );
          
           return true;
           }
          
           return false;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     147  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::IsStuckCondition>(  "IsStuck" );
          }

navigation2/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp

       1  // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "behaviortree_cpp_v3/condition_node.h"
          
          #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  PathExpiringTimerCondition::PathExpiringTimerCondition(  
      26   const std::string & condition_name,  
      27   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           period_(  1.0 ),  
           first_time_(  true )
          {
           getInput(  "seconds",   period_ );
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          }
          
      36  BT::NodeStatus PathExpiringTimerCondition::tick(   )
          {
           if (  first_time_ ) {
           getInput(  "path",   prev_path_ );
           first_time_ = false;
           start_ = node_->now(   );
           return BT::NodeStatus::FAILURE;
           }
          
           // Grab the new path
           nav_msgs::msg::Path path;
           getInput(  "path",   path );
          
           // Reset timer if the path has been updated
           if (  prev_path_ != path ) {
           prev_path_ = path;
           start_ = node_->now(   );
           }
          
           // Determine how long its been since we've started this iteration
           auto elapsed = node_->now(   ) - start_;
          
           // Now,   get that in seconds
           auto seconds = elapsed.seconds(   );
          
           if (  seconds < period_ ) {
           return BT::NodeStatus::FAILURE;
           }
          
           start_ = node_->now(   ); // Reset the timer
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      72  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::PathExpiringTimerCondition>(  "PathExpiringTimer" );
          }

navigation2/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "behaviortree_cpp_v3/condition_node.h"
          
          #include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
          
          namespace nav2_behavior_tree
          {
          
      26  TimeExpiredCondition::TimeExpiredCondition(  
      27   const std::string & condition_name,  
      28   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           period_(  1.0 )
          {
           getInput(  "seconds",   period_ );
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           start_ = node_->now(   );
          }
          
      37  BT::NodeStatus TimeExpiredCondition::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           start_ = node_->now(   );
           return BT::NodeStatus::FAILURE;
           }
          
           // Determine how long its been since we've started this iteration
           auto elapsed = node_->now(   ) - start_;
          
           // Now,   get that in seconds
           auto seconds = elapsed.seconds(   );
          
           if (  seconds < period_ ) {
           return BT::NodeStatus::FAILURE;
           }
          
           start_ = node_->now(   ); // Reset the timer
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      61  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>(  "TimeExpired" );
          }

navigation2/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp

       1  // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <chrono>
          #include <memory>
          
          #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          
          namespace nav2_behavior_tree
          {
          
      26  TransformAvailableCondition::TransformAvailableCondition(  
      27   const std::string & condition_name,  
      28   const BT::NodeConfiguration & conf )
          : BT::ConditionNode(  condition_name,   conf ),  
           was_found_(  false )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           tf_ = config(   ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer" );
          
           getInput(  "child",   child_frame_ );
           getInput(  "parent",   parent_frame_ );
          
           if (  child_frame_.empty(   ) || parent_frame_.empty(   ) ) {
           RCLCPP_FATAL(  
           node_->get_logger(   ),   "Child frame (  %s ) or parent frame (  %s ) were empty.",  
           child_frame_.c_str(   ),   parent_frame_.c_str(   ) );
           exit(  -1 );
           }
          
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Initialized an TransformAvailableCondition BT node" );
          }
          
      48  TransformAvailableCondition::~TransformAvailableCondition(   )
          {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Shutting down TransformAvailableCondition BT node" );
          }
          
      53  BT::NodeStatus TransformAvailableCondition::tick(   )
          {
           if (  was_found_ ) {
           return BT::NodeStatus::SUCCESS;
           }
          
           std::string tf_error;
           bool found = tf_->canTransform(  
           child_frame_,   parent_frame_,   tf2::TimePointZero,   &tf_error );
          
           if (  found ) {
           was_found_ = true;
           return BT::NodeStatus::SUCCESS;
           }
          
           RCLCPP_INFO(  
           node_->get_logger(   ),   "Transform from %s to %s was not found,   tf error: %s",  
           child_frame_.c_str(   ),   parent_frame_.c_str(   ),   tf_error.c_str(   ) );
          
           return BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      78  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::TransformAvailableCondition>(  "TransformAvailable" );
          }

navigation2/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stdexcept>
          #include <sstream>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
          
          namespace nav2_behavior_tree
          {
          
      24  PipelineSequence::PipelineSequence(  const std::string & name )
          : BT::ControlNode(  name,   {} )
          {
          }
          
      29  PipelineSequence::PipelineSequence(  
      30   const std::string & name,  
      31   const BT::NodeConfiguration & config )
          : BT::ControlNode(  name,   config )
          {
          }
          
      36  BT::NodeStatus PipelineSequence::tick(   )
          {
           for (  std::size_t i = 0; i < children_nodes_.size(   ); ++i ) {
           auto status = children_nodes_[i]->executeTick(   );
           switch (  status ) {
           case BT::NodeStatus::FAILURE:
           ControlNode::haltChildren(   );
           last_child_ticked_ = 0; // reset
           return status;
           case BT::NodeStatus::SUCCESS:
           // do nothing and continue on to the next child. If it is the last child
           // we'll exit the loop and hit the wrap-up code at the end of the method.
           break;
           case BT::NodeStatus::RUNNING:
           if (  i >= last_child_ticked_ ) {
           last_child_ticked_ = i;
           return status;
           }
           // else do nothing and continue on to the next child
           break;
           default:
           std::stringstream error_msg;
           error_msg << "Invalid node status. Received status " << status <<
           "from child " << children_nodes_[i]->name(   );
           throw std::runtime_error(  error_msg.str(   ) );
           }
           }
           // Wrap up.
           ControlNode::haltChildren(   );
           last_child_ticked_ = 0; // reset
           return BT::NodeStatus::SUCCESS;
          }
          
      69  void PipelineSequence::halt(   )
          {
           BT::ControlNode::halt(   );
           last_child_ticked_ = 0;
          }
          
          } // namespace nav2_behavior_tree
          
      77  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::PipelineSequence>(  "PipelineSequence" );
          }

navigation2/nav2_behavior_tree/plugins/control/recovery_node.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      21  RecoveryNode::RecoveryNode(  
      22   const std::string & name,  
      23   const BT::NodeConfiguration & conf )
          : BT::ControlNode::ControlNode(  name,   conf ),  
           current_child_idx_(  0 ),  
           number_of_retries_(  1 ),  
           retry_count_(  0 )
          {
           getInput(  "number_of_retries",   number_of_retries_ );
          }
          
      32  BT::NodeStatus RecoveryNode::tick(   )
          {
           const unsigned children_count = children_nodes_.size(   );
          
           if (  children_count != 2 ) {
           throw BT::BehaviorTreeException(  "Recovery Node '" + name(   ) + "' must only have 2 children." );
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           while (  current_child_idx_ < children_count && retry_count_ <= number_of_retries_ ) {
           TreeNode * child_node = children_nodes_[current_child_idx_];
           const BT::NodeStatus child_status = child_node->executeTick(   );
          
           if (  current_child_idx_ == 0 ) {
           switch (  child_status ) {
           case BT::NodeStatus::SUCCESS:
           {
           // reset node and return success when first child returns success
           halt(   );
           return BT::NodeStatus::SUCCESS;
           }
          
           case BT::NodeStatus::FAILURE:
           {
           if (  retry_count_ < number_of_retries_ ) {
           // halt first child and tick second child in next iteration
           ControlNode::haltChild(  0 );
           current_child_idx_++;
           break;
           } else {
           // reset node and return failure when max retries has been exceeded
           halt(   );
           return BT::NodeStatus::FAILURE;
           }
           }
          
           case BT::NodeStatus::RUNNING:
           {
           return BT::NodeStatus::RUNNING;
           }
          
           default:
           {
           throw BT::LogicError(  "A child node must never return IDLE" );
           }
           } // end switch
          
           } else if (  current_child_idx_ == 1 ) {
           switch (  child_status ) {
           case BT::NodeStatus::SUCCESS:
           {
           // halt second child,   increment recovery count,   and tick first child in next iteration
           ControlNode::haltChild(  1 );
           retry_count_++;
           current_child_idx_--;
           }
           break;
          
           case BT::NodeStatus::FAILURE:
           {
           // reset node and return failure if second child fails
           halt(   );
           return BT::NodeStatus::FAILURE;
           }
          
           case BT::NodeStatus::RUNNING:
           {
           return BT::NodeStatus::RUNNING;
           }
          
           default:
           {
           throw BT::LogicError(  "A child node must never return IDLE" );
           }
           } // end switch
           }
           } // end while loop
          
           // reset node and return failure
           halt(   );
           return BT::NodeStatus::FAILURE;
          }
          
     116  void RecoveryNode::halt(   )
          {
           ControlNode::halt(   );
           retry_count_ = 0;
           current_child_idx_ = 0;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     126  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::RecoveryNode>(  "RecoveryNode" );
          }

navigation2/nav2_behavior_tree/plugins/control/round_robin_node.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      22  RoundRobinNode::RoundRobinNode(  const std::string & name )
          : BT::ControlNode::ControlNode(  name,   {} )
          {
          }
          
      27  RoundRobinNode::RoundRobinNode(  
      28   const std::string & name,  
      29   const BT::NodeConfiguration & config )
          : BT::ControlNode(  name,   config )
          {
          }
          
      34  BT::NodeStatus RoundRobinNode::tick(   )
          {
           const auto num_children = children_nodes_.size(   );
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           while (  num_failed_children_ < num_children ) {
           TreeNode * child_node = children_nodes_[current_child_idx_];
           const BT::NodeStatus child_status = child_node->executeTick(   );
          
           switch (  child_status ) {
           case BT::NodeStatus::SUCCESS:
           {
           // Wrap around to the first child
           if (  ++current_child_idx_ >= num_children ) {
           current_child_idx_ = 0;
           }
           num_failed_children_ = 0;
           ControlNode::haltChildren(   );
           return BT::NodeStatus::SUCCESS;
           }
          
           case BT::NodeStatus::FAILURE:
           {
           if (  ++current_child_idx_ >= num_children ) {
           current_child_idx_ = 0;
           }
           num_failed_children_++;
           break;
           }
          
           case BT::NodeStatus::RUNNING:
           {
           return BT::NodeStatus::RUNNING;
           }
          
           default:
           {
           throw BT::LogicError(  "Invalid status return from BT node" );
           }
           }
           }
          
           halt(   );
           return BT::NodeStatus::FAILURE;
          }
          
      81  void RoundRobinNode::halt(   )
          {
           ControlNode::halt(   );
           current_child_idx_ = 0;
           num_failed_children_ = 0;
          }
          
          } // namespace nav2_behavior_tree
          
      90  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::RoundRobinNode>(  "RoundRobin" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/distance_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <string>
          #include <memory>
          #include <cmath>
          
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "tf2_ros/buffer.h"
          
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
          
          namespace nav2_behavior_tree
          {
          
      33  DistanceController::DistanceController(  
      34   const std::string & name,  
      35   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf ),  
           distance_(  1.0 ),  
           global_frame_(  "map" ),  
           robot_base_frame_(  "base_link" ),  
           first_time_(  false )
          {
           getInput(  "distance",   distance_ );
           getInput(  "global_frame",   global_frame_ );
           getInput(  "robot_base_frame",   robot_base_frame_ );
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           tf_ = config(   ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer" );
          
           node_->get_parameter(  "transform_tolerance",   transform_tolerance_ );
          }
          
      51  inline BT::NodeStatus DistanceController::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           // Reset the starting position since we're starting a new iteration of
           // the distance controller (  moving from IDLE to RUNNING )
           if (  !nav2_util::getCurrentPose(  
           start_pose_,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           return BT::NodeStatus::FAILURE;
           }
           first_time_ = true;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           // Determine distance travelled since we've started this iteration
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           return BT::NodeStatus::FAILURE;
           }
          
           // Get euclidean distance
           auto travelled = nav2_util::geometry_utils::euclidean_distance(  
           start_pose_.pose,   current_pose.pose );
          
           // The child gets ticked the first time through and every time the threshold
           // distance is crossed. In addition,   once the child begins to run,   it is
           // ticked each time 'til completion
           if (  first_time_ || (  child_node_->status(   ) == BT::NodeStatus::RUNNING ) ||
           travelled >= distance_ )
           {
           first_time_ = false;
           const BT::NodeStatus child_state = child_node_->executeTick(   );
          
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
          
           case BT::NodeStatus::SUCCESS:
           if (  !nav2_util::getCurrentPose(  
           start_pose_,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Current robot pose is not available." );
           return BT::NodeStatus::FAILURE;
           }
           return BT::NodeStatus::SUCCESS;
          
           case BT::NodeStatus::FAILURE:
           default:
           return BT::NodeStatus::FAILURE;
           }
           }
          
           return status(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     117  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::DistanceController>(  "DistanceController" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "behaviortree_cpp_v3/decorator_node.h"
          #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
          
          
          namespace nav2_behavior_tree
          {
          
      27  GoalUpdatedController::GoalUpdatedController(  
      28   const std::string & name,  
      29   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf )
          {
          }
          
      34  BT::NodeStatus GoalUpdatedController::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           // Reset since we're starting a new iteration of
           // the goal updated controller (  moving from IDLE to RUNNING )
          
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   goals_ );
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   goal_ );
          
           goal_was_updated_ = true;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           std::vector<geometry_msgs::msg::PoseStamped> current_goals;
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   current_goals );
           geometry_msgs::msg::PoseStamped current_goal;
           config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal",   current_goal );
          
           if (  goal_ != current_goal || goals_ != current_goals ) {
           goal_ = current_goal;
           goals_ = current_goals;
           goal_was_updated_ = true;
           }
          
           // The child gets ticked the first time through and any time the goal has
           // changed or preempted. In addition,   once the child begins to run,   it is ticked each time
           // 'til completion
           if (  (  child_node_->status(   ) == BT::NodeStatus::RUNNING ) || goal_was_updated_ ) {
           goal_was_updated_ = false;
           const BT::NodeStatus child_state = child_node_->executeTick(   );
          
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
          
           case BT::NodeStatus::SUCCESS:
           return BT::NodeStatus::SUCCESS;
          
           case BT::NodeStatus::FAILURE:
           default:
           return BT::NodeStatus::FAILURE;
           }
           }
          
           return status(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      85  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GoalUpdatedController>(  "GoalUpdatedController" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "behaviortree_cpp_v3/decorator_node.h"
          
          #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_behavior_tree
          {
          
          using std::placeholders::_1;
          
      31  GoalUpdater::GoalUpdater(  
      32   const std::string & name,  
      33   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
          
           std::string goal_updater_topic;
           node_->get_parameter_or<std::string>(  "goal_updater_topic",   goal_updater_topic,   "goal_update" );
          
           rclcpp::SubscriptionOptions sub_option;
           sub_option.callback_group = callback_group_;
           goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(  
           goal_updater_topic,  
           10,  
           std::bind(  &GoalUpdater::callback_updated_goal,   this,   _1 ),  
           sub_option );
          }
          
      54  inline BT::NodeStatus GoalUpdater::tick(   )
          {
           geometry_msgs::msg::PoseStamped goal;
          
           getInput(  "input_goal",   goal );
          
           callback_group_executor_.spin_some(   );
          
           if (  rclcpp::Time(  last_goal_received_.header.stamp ) > rclcpp::Time(  goal.header.stamp ) ) {
           goal = last_goal_received_;
           }
          
           setOutput(  "output_goal",   goal );
           return child_node_->executeTick(   );
          }
          
          void
      71  GoalUpdater::callback_updated_goal(  const geometry_msgs::msg::PoseStamped::SharedPtr msg )
          {
           last_goal_received_ = *msg;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      79  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::GoalUpdater>(  "GoalUpdater" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp

       1  // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          #include <vector>
          #include "nav2_util/geometry_utils.hpp"
          
          #include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
          
          namespace nav2_behavior_tree
          {
          
      25  PathLongerOnApproach::PathLongerOnApproach(  
      26   const std::string & name,  
      27   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          }
          
      33  bool PathLongerOnApproach::isPathUpdated(  
      34   nav_msgs::msg::Path & new_path,  
      35   nav_msgs::msg::Path & old_path )
          {
           return new_path != old_path && old_path.poses.size(   ) != 0 &&
           new_path.poses.size(   ) != 0 &&
           old_path.poses.back(   ) == new_path.poses.back(   );
          }
          
      42  bool PathLongerOnApproach::isRobotInGoalProximity(  
      43   nav_msgs::msg::Path & old_path,  
           double & prox_leng )
          {
           return nav2_util::geometry_utils::calculate_path_length(  old_path,   0 ) < prox_leng;
          }
          
      49  bool PathLongerOnApproach::isNewPathLonger(  
      50   nav_msgs::msg::Path & new_path,  
      51   nav_msgs::msg::Path & old_path,  
           double & length_factor )
          {
           return nav2_util::geometry_utils::calculate_path_length(  new_path,   0 ) >
           length_factor * nav2_util::geometry_utils::calculate_path_length(  
           old_path,   0 );
          }
          
      59  inline BT::NodeStatus PathLongerOnApproach::tick(   )
          {
           getInput(  "path",   new_path_ );
           getInput(  "prox_len",   prox_len_ );
           getInput(  "length_factor",   length_factor_ );
          
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           // Reset the starting point since we're starting a new iteration of
           // PathLongerOnApproach (  moving from IDLE to RUNNING )
           first_time_ = true;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           // Check if the path is updated and valid,   compare the old and the new path length,  
           // given the goal proximity and check if the new path is longer
           if (  isPathUpdated(  new_path_,   old_path_ ) && isRobotInGoalProximity(  old_path_,   prox_len_ ) &&
           isNewPathLonger(  new_path_,   old_path_,   length_factor_ ) && !first_time_ )
           {
           const BT::NodeStatus child_state = child_node_->executeTick(   );
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
           case BT::NodeStatus::SUCCESS:
           old_path_ = new_path_;
           return BT::NodeStatus::SUCCESS;
           case BT::NodeStatus::FAILURE:
           old_path_ = new_path_;
           return BT::NodeStatus::FAILURE;
           default:
           old_path_ = new_path_;
           return BT::NodeStatus::FAILURE;
           }
           }
           old_path_ = new_path_;
           first_time_ = false;
           return BT::NodeStatus::SUCCESS;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     101  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::PathLongerOnApproach>(  "PathLongerOnApproach" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/rate_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  RateController::RateController(  
      24   const std::string & name,  
      25   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf ),  
           first_time_(  false )
          {
           double hz = 1.0;
           getInput(  "hz",   hz );
           period_ = 1.0 / hz;
          }
          
      34  BT::NodeStatus RateController::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           // Reset the starting point since we're starting a new iteration of
           // the rate controller (  moving from IDLE to RUNNING )
           start_ = std::chrono::high_resolution_clock::now(   );
           first_time_ = true;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           // Determine how long its been since we've started this iteration
           auto now = std::chrono::high_resolution_clock::now(   );
           auto elapsed = now - start_;
          
           // Now,   get that in seconds
           typedef std::chrono::duration<float> float_seconds;
           auto seconds = std::chrono::duration_cast<float_seconds>(  elapsed );
          
           // The child gets ticked the first time through and any time the period has
           // expired. In addition,   once the child begins to run,   it is ticked each time
           // 'til completion
           if (  first_time_ || (  child_node_->status(   ) == BT::NodeStatus::RUNNING ) ||
           seconds.count(   ) >= period_ )
           {
           first_time_ = false;
           const BT::NodeStatus child_state = child_node_->executeTick(   );
          
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
          
           case BT::NodeStatus::SUCCESS:
           start_ = std::chrono::high_resolution_clock::now(   ); // Reset the timer
           return BT::NodeStatus::SUCCESS;
          
           case BT::NodeStatus::FAILURE:
           default:
           return BT::NodeStatus::FAILURE;
           }
           }
          
           return status(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      82  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::RateController>(  "RateController" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <string>
          
          #include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      23  SingleTrigger::SingleTrigger(  
      24   const std::string & name,  
      25   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf ),  
           first_time_(  true )
          {
          }
          
      31  BT::NodeStatus SingleTrigger::tick(   )
          {
           if (  status(   ) == BT::NodeStatus::IDLE ) {
           first_time_ = true;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           if (  first_time_ ) {
           const BT::NodeStatus child_state = child_node_->executeTick(   );
          
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
          
           case BT::NodeStatus::SUCCESS:
           first_time_ = false;
           return BT::NodeStatus::SUCCESS;
          
           case BT::NodeStatus::FAILURE:
           first_time_ = false;
           return BT::NodeStatus::FAILURE;
          
           default:
           first_time_ = false;
           return BT::NodeStatus::FAILURE;
           }
           }
          
           return BT::NodeStatus::FAILURE;
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
      66  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::SingleTrigger>(  "SingleTrigger" );
          }

navigation2/nav2_behavior_tree/plugins/decorator/speed_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          #include <vector>
          #include "nav2_util/geometry_utils.hpp"
          
          #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
          
          namespace nav2_behavior_tree
          {
          
      26  SpeedController::SpeedController(  
      27   const std::string & name,  
      28   const BT::NodeConfiguration & conf )
          : BT::DecoratorNode(  name,   conf ),  
           first_tick_(  false ),  
           period_(  1.0 ),  
           min_rate_(  0.1 ),  
           max_rate_(  1.0 ),  
           min_speed_(  0.0 ),  
           max_speed_(  0.5 )
          {
           node_ = config(   ).blackboard->get<rclcpp::Node::SharedPtr>(  "node" );
          
           getInput(  "min_rate",   min_rate_ );
           getInput(  "max_rate",   max_rate_ );
           getInput(  "min_speed",   min_speed_ );
           getInput(  "max_speed",   max_speed_ );
          
           if (  min_rate_ <= 0.0 || max_rate_ <= 0.0 ) {
           std::string err_msg = "SpeedController node cannot have rate <= 0.0";
           RCLCPP_FATAL(  node_->get_logger(   ),   err_msg.c_str(   ) );
           throw BT::BehaviorTreeException(  err_msg );
           }
          
           d_rate_ = max_rate_ - min_rate_;
           d_speed_ = max_speed_ - min_speed_;
          
           double duration = 0.3;
           getInput(  "filter_duration",   duration );
           std::string odom_topic;
           node_->get_parameter_or(  "odom_topic",   odom_topic,   std::string(  "odom" ) );
           odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(  node_,   duration,   odom_topic );
          }
          
      60  inline BT::NodeStatus SpeedController::tick(   )
          {
           auto current_goal = config(   ).blackboard->get<geometry_msgs::msg::PoseStamped>(  "goal" );
           auto current_goals =
           config(   ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals" );
          
           if (  goal_ != current_goal || goals_ != current_goals ) {
           // Reset state and set period to max since we have a new goal
           period_ = 1.0 / max_rate_;
           start_ = node_->now(   );
           first_tick_ = true;
           goal_ = current_goal;
           goals_ = current_goals;
           }
          
           setStatus(  BT::NodeStatus::RUNNING );
          
           auto elapsed = node_->now(   ) - start_;
          
           // The child gets ticked the first time through and any time the period has
           // expired. In addition,   once the child begins to run,   it is ticked each time
           // 'til completion
           if (  first_tick_ || (  child_node_->status(   ) == BT::NodeStatus::RUNNING ) ||
           elapsed.seconds(   ) >= period_ )
           {
           first_tick_ = false;
          
           // update period if the last period is exceeded
           if (  elapsed.seconds(   ) >= period_ ) {
           updatePeriod(   );
           start_ = node_->now(   );
           }
          
           const BT::NodeStatus child_state = child_node_->executeTick(   );
          
           switch (  child_state ) {
           case BT::NodeStatus::RUNNING:
           return BT::NodeStatus::RUNNING;
          
           case BT::NodeStatus::SUCCESS:
           return BT::NodeStatus::SUCCESS;
          
           case BT::NodeStatus::FAILURE:
           default:
           return BT::NodeStatus::FAILURE;
           }
           }
          
           return status(   );
          }
          
          } // namespace nav2_behavior_tree
          
          #include "behaviortree_cpp_v3/bt_factory.h"
     114  BT_REGISTER_NODES(  factory )
          {
           factory.registerNodeType<nav2_behavior_tree::SpeedController>(  "SpeedController" );
          }

navigation2/nav2_behavior_tree/src/behavior_tree_engine.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Florian Gramss
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_behavior_tree/behavior_tree_engine.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "behaviortree_cpp_v3/utils/shared_library.h"
          
          namespace nav2_behavior_tree
          {
          
      28  BehaviorTreeEngine::BehaviorTreeEngine(  const std::vector<std::string> & plugin_libraries )
          {
           BT::SharedLibrary loader;
           for (  const auto & p : plugin_libraries ) {
           factory_.registerFromPlugin(  loader.getOSName(  p ) );
           }
          }
          
          BtStatus
      37  BehaviorTreeEngine::run(  
      38   BT::Tree * tree,  
      39   std::function<void(   )> onLoop,  
      40   std::function<bool(   )> cancelRequested,  
      41   std::chrono::milliseconds loopTimeout )
          {
           rclcpp::WallRate loopRate(  loopTimeout );
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           // Loop until something happens with ROS or the node completes
           try {
           while (  rclcpp::ok(   ) && result == BT::NodeStatus::RUNNING ) {
           if (  cancelRequested(   ) ) {
           tree->rootNode(   )->halt(   );
           return BtStatus::CANCELED;
           }
          
           result = tree->tickRoot(   );
          
           onLoop(   );
          
           loopRate.sleep(   );
           }
           } catch (  const std::exception & ex ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "BehaviorTreeEngine" ),  
           "Behavior tree threw exception: %s. Exiting with failure.",   ex.what(   ) );
           return BtStatus::FAILED;
           }
          
           return (  result == BT::NodeStatus::SUCCESS ) ? BtStatus::SUCCEEDED : BtStatus::FAILED;
          }
          
          BT::Tree
      71  BehaviorTreeEngine::createTreeFromText(  
      72   const std::string & xml_string,  
      73   BT::Blackboard::Ptr blackboard )
          {
           return factory_.createTreeFromText(  xml_string,   blackboard );
          }
          
          BT::Tree
      79  BehaviorTreeEngine::createTreeFromFile(  
      80   const std::string & file_path,  
      81   BT::Blackboard::Ptr blackboard )
          {
           return factory_.createTreeFromFile(  file_path,   blackboard );
          }
          
          // In order to re-run a Behavior Tree,   we must be able to reset all nodes to the initial state
          void
      88  BehaviorTreeEngine::haltAllActions(  BT::TreeNode * root_node )
          {
           if (  !root_node ) {
           return;
           }
          
           // this halt signal should propagate through the entire tree.
           root_node->halt(   );
          
           // but,   just in case...
           auto visitor = [](  BT::TreeNode * node ) {
           if (  node->status(   ) == BT::NodeStatus::RUNNING ) {
           node->halt(   );
           }
           };
           BT::applyRecursiveVisitor(  root_node,   visitor );
          }
          
          } // namespace nav2_behavior_tree

navigation2/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
          
      26  class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>
          {
          public:
      29   BackUpActionServer(   )
           : TestActionServer(  "backup" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
           goal_handle )
           override
           {
           nav2_msgs::action::BackUp::Result::SharedPtr result =
           std::make_shared<nav2_msgs::action::BackUp::Result>(   );
           bool return_success = getReturnSuccess(   );
           if (  return_success ) {
           goal_handle->succeed(  result );
           } else {
           goal_handle->abort(  result );
           }
           }
          };
          
          class BackUpActionTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "backup_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::BackUpAction>(  
           name,   "backup",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::BackUpAction>(  "BackUp",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<BackUpActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr BackUpActionTestFixture::node_ = nullptr;
          std::shared_ptr<BackUpActionServer> BackUpActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * BackUpActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> BackUpActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> BackUpActionTestFixture::tree_ = nullptr;
          
          TEST_F(  BackUpActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <BackUp />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "backup_dist" ),   0.15 );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "backup_speed" ),   0.025 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <BackUp backup_dist="2" backup_speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "backup_dist" ),   2.0 );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "backup_speed" ),   0.26 );
          }
          
          TEST_F(  BackUpActionTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <BackUp backup_dist="2" backup_speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
          
           auto goal = action_server_->getCurrentGoal(   );
           EXPECT_EQ(  goal->target.x,   2.0 );
           EXPECT_EQ(  goal->speed,   0.26f );
          }
          
          TEST_F(  BackUpActionTestFixture,   test_failure )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <BackUp backup_dist="2" backup_speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           action_server_->setReturnSuccess(  false );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
          
           auto goal = action_server_->getCurrentGoal(   );
           EXPECT_EQ(  goal->target.x,   2.0 );
           EXPECT_EQ(  goal->speed,   0.26f );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           BackUpActionTestFixture::action_server_ = std::make_shared<BackUpActionServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  BackUpActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
          #include "lifecycle_msgs/srv/change_state.hpp"
          
      26  class CancelBackUpServer : public TestActionServer<nav2_msgs::action::BackUp>
          {
          public:
      29   CancelBackUpServer(   )
           : TestActionServer(  "back_up" )
           {}
          
          protected:
      34   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
           goal_handle )
           {
           while (  !goal_handle->is_canceling(   ) ) {
           // BackUping here until goal cancels
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
           }
           }
          };
          
      45  class CancelBackUpActionTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "cancel_back_up_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(  
           node_,   "back_up" );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::BackUpCancel>(  
           name,   "back_up",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::BackUpCancel>(  "CancelBackUp",   builder );
           }
          
      80   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           client_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<CancelBackUpServer> action_server_;
      96   static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>> client_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr CancelBackUpActionTestFixture::node_ = nullptr;
          std::shared_ptr<CancelBackUpServer>
          CancelBackUpActionTestFixture::action_server_ = nullptr;
          std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>>
          CancelBackUpActionTestFixture::client_ = nullptr;
          
          BT::NodeConfiguration * CancelBackUpActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          CancelBackUpActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> CancelBackUpActionTestFixture::tree_ = nullptr;
          
          TEST_F(  CancelBackUpActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <CancelBackUp name="BackUpCancel"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::BackUp>::SendGoalOptions(   );
          
           // Creating a dummy goal_msg
           auto goal_msg = nav2_msgs::action::BackUp::Goal(   );
          
           // Setting target pose
           goal_msg.target.x = 0.5;
          
           // BackUping for server and sending a goal
           client_->wait_for_action_server(   );
           client_->async_send_goal(  goal_msg,   send_goal_options );
          
           // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
          
           // Executing tick
           tree_->rootNode(   )->executeTick(   );
          
           // BT node should return success,   once when the goal is cancelled
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Adding another test case to check if the goal is infact cancelling
           EXPECT_EQ(  action_server_->isGoalCancelled(   ),   true );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and back_up on new thread
           CancelBackUpActionTestFixture::action_server_ = std::make_shared<CancelBackUpServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  CancelBackUpActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <vector>
          #include <string>
          #include <chrono>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "nav2_behavior_tree/bt_action_node.hpp"
          
          #include "test_msgs/action/fibonacci.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          using namespace std::placeholders; // NOLINT
          
      34  class FibonacciActionServer : public rclcpp::Node
          {
          public:
           FibonacciActionServer(   )
           : rclcpp::Node(  "fibonacci_node",   rclcpp::NodeOptions(   ) ),  
           sleep_duration_(  0ms )
           {
           this->action_server_ = rclcpp_action::create_server<test_msgs::action::Fibonacci>(  
           this->get_node_base_interface(   ),  
           this->get_node_clock_interface(   ),  
           this->get_node_logging_interface(   ),  
           this->get_node_waitables_interface(   ),  
           "fibonacci",  
           std::bind(  &FibonacciActionServer::handle_goal,   this,   _1,   _2 ),  
           std::bind(  &FibonacciActionServer::handle_cancel,   this,   _1 ),  
           std::bind(  &FibonacciActionServer::handle_accepted,   this,   _1 ) );
           }
          
           void setHandleGoalSleepDuration(  std::chrono::milliseconds sleep_duration )
           {
           sleep_duration_ = sleep_duration;
           }
          
          protected:
           rclcpp_action::GoalResponse handle_goal(  
           const rclcpp_action::GoalUUID &,  
           std::shared_ptr<const test_msgs::action::Fibonacci::Goal> )
           {
           if (  sleep_duration_ > 0ms ) {
           std::this_thread::sleep_for(  sleep_duration_ );
           }
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
           }
          
           rclcpp_action::CancelResponse handle_cancel(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> )
           {
           return rclcpp_action::CancelResponse::ACCEPT;
           }
          
           void handle_accepted(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle )
           {
           // this needs to return quickly to avoid blocking the executor,   so spin up a new thread
           if (  handle ) {
           const auto goal = handle->get_goal(   );
           auto result = std::make_shared<test_msgs::action::Fibonacci::Result>(   );
          
           if (  goal->order < 0 ) {
           handle->abort(  result );
           return;
           }
          
           auto & sequence = result->sequence;
           sequence.push_back(  0 );
           sequence.push_back(  1 );
          
           for (  int i = 1; (  i < goal->order ) && rclcpp::ok(   ); ++i ) {
           sequence.push_back(  sequence[i] + sequence[i - 1] );
           }
          
           handle->succeed(  result );
           }
           }
          
          protected:
           rclcpp_action::Server<test_msgs::action::Fibonacci>::SharedPtr action_server_;
           std::chrono::milliseconds sleep_duration_;
          };
          
          class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>
          {
          public:
           FibonacciAction(  
           const std::string & xml_tag_name,  
           const BT::NodeConfiguration & conf )
           : nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>(  xml_tag_name,   "fibonacci",   conf )
           {}
          
           void on_tick(   ) override
           {
           getInput(  "order",   goal_.order );
           }
          
           BT::NodeStatus on_success(   ) override
           {
           config(   ).blackboard->set<std::vector<int>>(  "sequence",   result_.result->sequence );
           return BT::NodeStatus::SUCCESS;
           }
          
           static BT::PortsList providedPorts(   )
           {
           return providedBasicPorts(  {BT::InputPort<int>(  "order",   "Fibonacci order" )} );
           }
          };
          
          class BTActionNodeTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "bt_action_node_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  "server_timeout",   20ms );
           config_->blackboard->set<std::chrono::milliseconds>(  "bt_loop_duration",   10ms );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<FibonacciAction>(  name,   config );
           };
          
           factory_->registerBuilder<FibonacciAction>(  "Fibonacci",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           // initialize action server and spin on new thread
           action_server_ = std::make_shared<FibonacciActionServer>(   );
           server_thread_ = std::make_shared<std::thread>(  
           [](   ) {
           while (  rclcpp::ok(   ) && BTActionNodeTestFixture::action_server_ != nullptr ) {
           rclcpp::spin_some(  BTActionNodeTestFixture::action_server_ );
           std::this_thread::sleep_for(  100ns );
           }
           } );
           }
          
           void TearDown(   ) override
           {
           action_server_.reset(   );
           tree_.reset(   );
           server_thread_->join(   );
           server_thread_.reset(   );
           }
          
           static std::shared_ptr<FibonacciActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
           static std::shared_ptr<std::thread> server_thread_;
          };
          
          rclcpp::Node::SharedPtr BTActionNodeTestFixture::node_ = nullptr;
          std::shared_ptr<FibonacciActionServer> BTActionNodeTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * BTActionNodeTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> BTActionNodeTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> BTActionNodeTestFixture::tree_ = nullptr;
          std::shared_ptr<std::thread> BTActionNodeTestFixture::server_thread_ = nullptr;
          
          TEST_F(  BTActionNodeTestFixture,   test_server_timeout_success )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Fibonacci order="5" />
           </BehaviorTree>
           </root> )";
          
           // the server timeout is larger than the goal handling duration
           config_->blackboard->set<std::chrono::milliseconds>(  "server_timeout",   20ms );
           config_->blackboard->set<std::chrono::milliseconds>(  "bt_loop_duration",   10ms );
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // setting a small action server goal handling duration
           action_server_->setHandleGoalSleepDuration(  2ms );
          
           // to keep track of the number of ticks it took to reach a terminal result
           int ticks = 0;
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           // BT loop execution rate
           rclcpp::WallRate loopRate(  10ms );
          
           // main BT execution loop
           while (  rclcpp::ok(   ) && result == BT::NodeStatus::RUNNING ) {
           result = tree_->tickRoot(   );
           ticks++;
           loopRate.sleep(   );
           }
          
           // get calculated fibonacci sequence from blackboard
           auto sequence = config_->blackboard->get<std::vector<int>>(  "sequence" );
          
           // expected fibonacci sequence for order 5
           std::vector<int> expected = {0,   1,   1,   2,   3,   5};
          
           // since the server timeout was larger than the action server goal handling duration
           // the BT should have succeeded
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          
           // checking the output fibonacci sequence
           EXPECT_EQ(  sequence.size(   ),   expected.size(   ) );
           for (  size_t i = 0; i < expected.size(   ); ++i ) {
           EXPECT_EQ(  sequence[i],   expected[i] );
           }
          
           // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
           // the new cycle
          
           // halt BT for a new execution cycle
           tree_->haltTree(   );
          
           // setting a large action server goal handling duration
           action_server_->setHandleGoalSleepDuration(  100ms );
          
           // reset state variables
           ticks = 0;
           result = BT::NodeStatus::RUNNING;
          
           // main BT execution loop
           while (  rclcpp::ok(   ) && result == BT::NodeStatus::RUNNING ) {
           result = tree_->tickRoot(   );
           ticks++;
           loopRate.sleep(   );
           }
          
           // since the server timeout was smaller than the action server goal handling duration
           // the BT should have failed
           EXPECT_EQ(  result,   BT::NodeStatus::FAILURE );
          
           // since the server timeout is 20ms and bt loop duration is 10ms,   number of ticks should be 2
           EXPECT_EQ(  ticks,   2 );
          }
          
          TEST_F(  BTActionNodeTestFixture,   test_server_timeout_failure )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Fibonacci order="2" />
           </BehaviorTree>
           </root> )";
          
           // setting a server timeout smaller than the time the action server will take to accept the goal
           // to simulate a server timeout scenario
           config_->blackboard->set<std::chrono::milliseconds>(  "server_timeout",   90ms );
           config_->blackboard->set<std::chrono::milliseconds>(  "bt_loop_duration",   10ms );
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // the action server will take 100ms before accepting the goal
           action_server_->setHandleGoalSleepDuration(  100ms );
          
           // to keep track of the number of ticks it took to reach a terminal result
           int ticks = 0;
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           // BT loop execution rate
           rclcpp::WallRate loopRate(  10ms );
          
           // main BT execution loop
           while (  rclcpp::ok(   ) && result == BT::NodeStatus::RUNNING ) {
           result = tree_->tickRoot(   );
           ticks++;
           loopRate.sleep(   );
           }
          
           // since the server timeout was smaller than the action server goal handling duration
           // the BT should have failed
           EXPECT_EQ(  result,   BT::NodeStatus::FAILURE );
          
           // since the server timeout is 90ms and bt loop duration is 10ms,   number of ticks should be 9
           EXPECT_EQ(  ticks,   9 );
          
           // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
           // the new cycle
          
           // halt BT for a new execution cycle
           tree_->haltTree(   );
          
           // setting a small action server goal handling duration
           action_server_->setHandleGoalSleepDuration(  25ms );
          
           // reset state variables
           ticks = 0;
           result = BT::NodeStatus::RUNNING;
          
           // main BT execution loop
           while (  rclcpp::ok(   ) && result == BT::NodeStatus::RUNNING ) {
           result = tree_->tickRoot(   );
           ticks++;
           loopRate.sleep(   );
           }
          
           // since the server timeout was smaller than the action server goal handling duration
           // the BT should have failed
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_service.hpp"
          #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
          
      26  class ClearEntireCostmapService : public TestService<nav2_msgs::srv::ClearEntireCostmap>
          {
          public:
      29   ClearEntireCostmapService(   )
           : TestService(  "clear_entire_costmap" )
           {}
          };
          
      34  class ClearEntireCostmapServiceTestFixture : public ::testing::Test
          {
          public:
      37   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "clear_entire_costmap_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           factory_->registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>(  "ClearEntireCostmap" );
           }
          
      62   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ClearEntireCostmapService> server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr;
          std::shared_ptr<ClearEntireCostmapService> ClearEntireCostmapServiceTestFixture::server_ = nullptr;
          BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> ClearEntireCostmapServiceTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> ClearEntireCostmapServiceTestFixture::tree_ = nullptr;
          
          TEST_F(  ClearEntireCostmapServiceTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ClearEntireCostmap service_name="clear_entire_costmap"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
           EXPECT_EQ(  tree_->rootNode(   )->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
          }
          
          class ClearCostmapExceptRegionService : public TestService<nav2_msgs::srv::ClearCostmapExceptRegion>
          {
          public:
           ClearCostmapExceptRegionService(   )
           : TestService(  "clear_costmap_except_region" )
           {}
          };
          
          class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "clear_costmap_except_region_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           factory_->registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(  
           "ClearCostmapExceptRegion" );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ClearCostmapExceptRegionService> server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr
          ClearCostmapExceptRegionServiceTestFixture::node_ = nullptr;
          std::shared_ptr<ClearCostmapExceptRegionService>
          ClearCostmapExceptRegionServiceTestFixture::server_ = nullptr;
          BT::NodeConfiguration
          * ClearCostmapExceptRegionServiceTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          ClearCostmapExceptRegionServiceTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree>
          ClearCostmapExceptRegionServiceTestFixture::tree_ = nullptr;
          
          TEST_F(  ClearCostmapExceptRegionServiceTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ClearCostmapExceptRegion service_name="clear_costmap_except_region"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
           EXPECT_EQ(  tree_->rootNode(   )->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
          }
          //******************************************
          class ClearCostmapAroundRobotService : public TestService<nav2_msgs::srv::ClearCostmapAroundRobot>
          {
          public:
           ClearCostmapAroundRobotService(   )
           : TestService(  "clear_costmap_around_robot" )
           {}
          };
          
          class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "clear_costmap_around_robot_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           factory_->registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(  
           "ClearCostmapAroundRobot" );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ClearCostmapAroundRobotService> server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr
          ClearCostmapAroundRobotServiceTestFixture::node_ = nullptr;
          std::shared_ptr<ClearCostmapAroundRobotService>
          ClearCostmapAroundRobotServiceTestFixture::server_ = nullptr;
          BT::NodeConfiguration
          * ClearCostmapAroundRobotServiceTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          ClearCostmapAroundRobotServiceTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree>
          ClearCostmapAroundRobotServiceTestFixture::tree_ = nullptr;
          
          TEST_F(  ClearCostmapAroundRobotServiceTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ClearCostmapAroundRobot service_name="clear_costmap_around_robot"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
           EXPECT_EQ(  tree_->rootNode(   )->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize service and spin on new thread
           ClearEntireCostmapServiceTestFixture::server_ = std::make_shared<ClearEntireCostmapService>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  ClearEntireCostmapServiceTestFixture::server_ );
           } );
          
           ClearCostmapExceptRegionServiceTestFixture::server_ =
           std::make_shared<ClearCostmapExceptRegionService>(   );
           std::thread server_thread_except_region(  [](   ) {
           rclcpp::spin(  ClearCostmapExceptRegionServiceTestFixture::server_ );
           } );
          
           ClearCostmapAroundRobotServiceTestFixture::server_ =
           std::make_shared<ClearCostmapAroundRobotService>(   );
           std::thread server_thread_around_robot(  [](   ) {
           rclcpp::spin(  ClearCostmapAroundRobotServiceTestFixture::server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
           server_thread_except_region.join(   );
           server_thread_around_robot.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          #include <vector>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
          
      30  class ComputePathThroughPosesActionServer
      31   : public TestActionServer<nav2_msgs::action::ComputePathThroughPoses>
          {
          public:
      34   ComputePathThroughPosesActionServer(   )
           : TestActionServer(  "compute_path_through_poses" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathThroughPoses>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::ComputePathThroughPoses::Result>(   );
           result->path.poses.resize(  2 );
           result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x;
           if (  goal->use_start ) {
           result->path.poses[0].pose.position.x = goal->start.pose.position.x;
           } else {
           result->path.poses[0].pose.position.x = 0.0;
           }
           goal_handle->succeed(  result );
           }
          };
          
          class ComputePathThroughPosesActionTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "compute_path_through_poses_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(  
           name,   "compute_path_through_poses",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(  
           "ComputePathThroughPoses",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ComputePathThroughPosesActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr ComputePathThroughPosesActionTestFixture::node_ = nullptr;
          std::shared_ptr<ComputePathThroughPosesActionServer>
          ComputePathThroughPosesActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * ComputePathThroughPosesActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> ComputePathThroughPosesActionTestFixture::factory_ =
           nullptr;
          std::shared_ptr<BT::Tree> ComputePathThroughPosesActionTestFixture::tree_ = nullptr;
          
          TEST_F(  ComputePathThroughPosesActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           std::vector<geometry_msgs::msg::PoseStamped> goals;
           goals.resize(  1 );
           goals[0].pose.position.x = 1.0;
           config_->blackboard->set(  "goals",   goals );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // the goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<std::string>(  "planner_id" ),   std::string(  "GridBased" ) );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goals[0].pose.position.x,   1.0 );
           EXPECT_FALSE(  action_server_->getCurrentGoal(   )->use_start );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->planner_id,   std::string(  "GridBased" ) );
          
           // check if returned path is correct
           nav_msgs::msg::Path path;
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   0.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   1.0 );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal
           goals[0].pose.position.x = -2.5;
           config_->blackboard->set(  "goals",   goals );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goals[0].pose.position.x,   -2.5 );
          
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   0.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   -2.5 );
          }
          
          TEST_F(  ComputePathThroughPosesActionTestFixture,   test_tick_use_start )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ComputePathThroughPoses goals="{goals}" start="{start}" path="{path}" planner_id="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new start and set it on blackboard
           geometry_msgs::msg::PoseStamped start;
           start.header.stamp = node_->now(   );
           start.pose.position.x = 2.0;
           config_->blackboard->set(  "start",   start );
          
           // create new goal and set it on blackboard
           std::vector<geometry_msgs::msg::PoseStamped> goals;
           goals.resize(  1 );
           goals[0].pose.position.x = 1.0;
           config_->blackboard->set(  "goals",   goals );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // the goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<std::string>(  "planner_id" ),   std::string(  "GridBased" ) );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goals[0].pose.position.x,   1.0 );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->start.pose.position.x,   2.0 );
           EXPECT_TRUE(  action_server_->getCurrentGoal(   )->use_start );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->planner_id,   std::string(  "GridBased" ) );
          
           // check if returned path is correct
           nav_msgs::msg::Path path;
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   2.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   1.0 );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal and new start
           goals[0].pose.position.x = -2.5;
           start.pose.position.x = -1.5;
           config_->blackboard->set(  "goals",   goals );
           config_->blackboard->set(  "start",   start );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goals[0].pose.position.x,   -2.5 );
          
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   -1.5 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   -2.5 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           ComputePathThroughPosesActionTestFixture::action_server_ =
           std::make_shared<ComputePathThroughPosesActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  ComputePathThroughPosesActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
          
      29  class ComputePathToPoseActionServer : public TestActionServer<nav2_msgs::action::ComputePathToPose>
          {
          public:
      32   ComputePathToPoseActionServer(   )
           : TestActionServer(  "compute_path_to_pose" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>(   );
           result->path.poses.resize(  2 );
           result->path.poses[1].pose.position.x = goal->goal.pose.position.x;
           if (  goal->use_start ) {
           result->path.poses[0].pose.position.x = goal->start.pose.position.x;
           } else {
           result->path.poses[0].pose.position.x = 0.0;
           }
           goal_handle->succeed(  result );
           }
          };
          
          class ComputePathToPoseActionTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "compute_path_to_pose_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(  
           name,   "compute_path_to_pose",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(  
           "ComputePathToPose",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ComputePathToPoseActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr ComputePathToPoseActionTestFixture::node_ = nullptr;
          std::shared_ptr<ComputePathToPoseActionServer>
          ComputePathToPoseActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * ComputePathToPoseActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> ComputePathToPoseActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> ComputePathToPoseActionTestFixture::tree_ = nullptr;
          
          TEST_F(  ComputePathToPoseActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           goal.pose.position.x = 1.0;
           config_->blackboard->set(  "goal",   goal );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // the goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<std::string>(  "planner_id" ),   std::string(  "GridBased" ) );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goal.pose.position.x,   1.0 );
           EXPECT_FALSE(  action_server_->getCurrentGoal(   )->use_start );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->planner_id,   std::string(  "GridBased" ) );
          
           // check if returned path is correct
           nav_msgs::msg::Path path;
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   0.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   1.0 );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal
           goal.pose.position.x = -2.5;
           config_->blackboard->set(  "goal",   goal );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goal.pose.position.x,   -2.5 );
          
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   0.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   -2.5 );
          }
          
          TEST_F(  ComputePathToPoseActionTestFixture,   test_tick_use_start )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new start and set it on blackboard
           geometry_msgs::msg::PoseStamped start;
           start.header.stamp = node_->now(   );
           start.pose.position.x = 2.0;
           config_->blackboard->set(  "start",   start );
          
           // create new goal and set it on blackboard
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           goal.pose.position.x = 1.0;
           config_->blackboard->set(  "goal",   goal );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // the goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<std::string>(  "planner_id" ),   std::string(  "GridBased" ) );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goal.pose.position.x,   1.0 );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->start.pose.position.x,   2.0 );
           EXPECT_TRUE(  action_server_->getCurrentGoal(   )->use_start );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->planner_id,   std::string(  "GridBased" ) );
          
           // check if returned path is correct
           nav_msgs::msg::Path path;
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   2.0 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   1.0 );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal and new start
           goal.pose.position.x = -2.5;
           start.pose.position.x = -1.5;
           config_->blackboard->set(  "goal",   goal );
           config_->blackboard->set(  "start",   start );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->goal.pose.position.x,   -2.5 );
          
           config_->blackboard->get<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  path.poses.size(   ),   2u );
           EXPECT_EQ(  path.poses[0].pose.position.x,   -1.5 );
           EXPECT_EQ(  path.poses[1].pose.position.x,   -2.5 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           ComputePathToPoseActionTestFixture::action_server_ =
           std::make_shared<ComputePathToPoseActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  ComputePathToPoseActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
          #include "lifecycle_msgs/srv/change_state.hpp"
          
      26  class CancelControllerServer : public TestActionServer<nav2_msgs::action::FollowPath>
          {
          public:
      29   CancelControllerServer(   )
           : TestActionServer(  "follow_path" )
           {}
          
          protected:
      34   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>>
           goal_handle )
           {
           while (  !goal_handle->is_canceling(   ) ) {
           // waiting here until goal cancels
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
           }
           }
          };
          
      45  class CancelControllerActionTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "cancel_control_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           client_ = rclcpp_action::create_client<nav2_msgs::action::FollowPath>(  
           node_,   "follow_path" );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::ControllerCancel>(  
           name,   "follow_path",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::ControllerCancel>(  "CancelControl",   builder );
           }
          
      80   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           client_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<CancelControllerServer> action_server_;
      96   static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>> client_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr CancelControllerActionTestFixture::node_ = nullptr;
          std::shared_ptr<CancelControllerServer>
          CancelControllerActionTestFixture::action_server_ = nullptr;
          std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>>
          CancelControllerActionTestFixture::client_ = nullptr;
          
          BT::NodeConfiguration * CancelControllerActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          CancelControllerActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> CancelControllerActionTestFixture::tree_ = nullptr;
          
          TEST_F(  CancelControllerActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <CancelControl name="ControlCancel"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::FollowPath>::SendGoalOptions(   );
          
           // Creating a dummy goal_msg
           auto goal_msg = nav2_msgs::action::FollowPath::Goal(   );
          
           // Waiting for server and sending a goal
           client_->wait_for_action_server(   );
           client_->async_send_goal(  goal_msg,   send_goal_options );
          
           // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
          
           // Executing tick
           tree_->rootNode(   )->executeTick(   );
          
           // BT node should return success,   once when the goal is cancelled
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Adding another test case to check if the goal is infact cancelling
           EXPECT_EQ(  action_server_->isGoalCancelled(   ),   true );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           CancelControllerActionTestFixture::action_server_ = std::make_shared<CancelControllerServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  CancelControllerActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <memory>
          #include <set>
          #include <string>
          
          #include "../../test_action_server.hpp"
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "std_msgs/msg/string.hpp"
          
      28  class ControllerSelectorTestFixture : public ::testing::Test
          {
          public:
      31   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "controller_selector_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
          
           BT::NodeBuilder builder = [](  const std::string & name,   const BT::NodeConfiguration & config ) {
           return std::make_unique<nav2_behavior_tree::ControllerSelector>(  name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::ControllerSelector>(  
           "ControllerSelector",  
           builder );
           }
          
      52   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr ControllerSelectorTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> ControllerSelectorTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> ControllerSelectorTestFixture::tree_ = nullptr;
          
          TEST_F(  ControllerSelectorTestFixture,   test_custom_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ControllerSelector selected_controller="{selected_controller}" default_controller="DWB" topic_name="controller_selector_custom_topic_name"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_controller_result;
           config_->blackboard->get(  "selected_controller",   selected_controller_result );
          
           EXPECT_EQ(  selected_controller_result,   "DWB" );
          
           std_msgs::msg::String selected_controller_cmd;
          
           selected_controller_cmd.data = "DWC";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto controller_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "controller_selector_custom_topic_name",   qos );
          
           // publish a few updates of the selected_controller
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           controller_selector_pub->publish(  selected_controller_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check controller updated
           config_->blackboard->get(  "selected_controller",   selected_controller_result );
           EXPECT_EQ(  "DWC",   selected_controller_result );
          }
          
          TEST_F(  ControllerSelectorTestFixture,   test_default_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ControllerSelector selected_controller="{selected_controller}" default_controller="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_controller_result;
           config_->blackboard->get(  "selected_controller",   selected_controller_result );
          
           EXPECT_EQ(  selected_controller_result,   "GridBased" );
          
           std_msgs::msg::String selected_controller_cmd;
          
           selected_controller_cmd.data = "RRT";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto controller_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "controller_selector",   qos );
          
           // publish a few updates of the selected_controller
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           controller_selector_pub->publish(  selected_controller_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check controller updated
           config_->blackboard->get(  "selected_controller",   selected_controller_result );
           EXPECT_EQ(  "RRT",   selected_controller_result );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
          
      26  class DriveOnHeadingActionServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
          {
          public:
      29   DriveOnHeadingActionServer(   )
           : TestActionServer(  "drive_on_heading" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
           <nav2_msgs::action::DriveOnHeading>>
           goal_handle )
           override
           {
           nav2_msgs::action::DriveOnHeading::Result::SharedPtr result =
           std::make_shared<nav2_msgs::action::DriveOnHeading::Result>(   );
           bool return_success = getReturnSuccess(   );
           if (  return_success ) {
           goal_handle->succeed(  result );
           } else {
           goal_handle->abort(  result );
           }
           }
          };
          
          class DriveOnHeadingActionTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "drive_on_heading_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(  
           name,   "drive_on_heading",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>(  "DriveOnHeading",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<DriveOnHeadingActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr DriveOnHeadingActionTestFixture::node_ = nullptr;
          std::shared_ptr<DriveOnHeadingActionServer>
          DriveOnHeadingActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * DriveOnHeadingActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> DriveOnHeadingActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> DriveOnHeadingActionTestFixture::tree_ = nullptr;
          
          TEST_F(  DriveOnHeadingActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <DriveOnHeading />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "dist_to_travel" ),   0.15 );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "speed" ),   0.025 );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "time_allowance" ),   10.0 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <DriveOnHeading dist_to_travel="2" speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "dist_to_travel" ),   2.0 );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "speed" ),   0.26 );
          }
          
          TEST_F(  DriveOnHeadingActionTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <DriveOnHeading dist_to_travel="2" speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           auto goal = action_server_->getCurrentGoal(   );
           EXPECT_EQ(  goal->target.x,   2.0 );
           EXPECT_EQ(  goal->speed,   0.26f );
          }
          
          TEST_F(  DriveOnHeadingActionTestFixture,   test_failure )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <DriveOnHeading dist_to_travel="2" speed="0.26" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           action_server_->setReturnSuccess(  false );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
          
           auto goal = action_server_->getCurrentGoal(   );
           EXPECT_EQ(  goal->target.x,   2.0 );
           EXPECT_EQ(  goal->speed,   0.26f );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           DriveOnHeadingActionTestFixture::action_server_ = std::make_shared<DriveOnHeadingActionServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  DriveOnHeadingActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp

          // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
          #include "lifecycle_msgs/srv/change_state.hpp"
          
      26  class CancelDriveOnHeadingServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
          {
          public:
      29   CancelDriveOnHeadingServer(   )
           : TestActionServer(  "drive_on_heading_cancel" )
           {}
          
          protected:
      34   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
           <nav2_msgs::action::DriveOnHeading>>
           goal_handle )
           {
           while (  !goal_handle->is_canceling(   ) ) {
           // DriveOnHeadingCancel here until goal cancels
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
           }
           }
          };
          
      46  class CancelDriveOnHeadingTestFixture : public ::testing::Test
          {
          public:
      49   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "cancel_drive_on_heading_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           client_ = rclcpp_action::create_client<nav2_msgs::action::DriveOnHeading>(  
           node_,   "drive_on_heading_cancel" );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(  
           name,   "drive_on_heading_cancel",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(  
           "CancelDriveOnHeading",  
           builder );
           }
          
      83   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           client_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<CancelDriveOnHeadingServer> action_server_;
      99   static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>> client_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr CancelDriveOnHeadingTestFixture::node_ = nullptr;
          std::shared_ptr<CancelDriveOnHeadingServer>
          CancelDriveOnHeadingTestFixture::action_server_ = nullptr;
          std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>>
          CancelDriveOnHeadingTestFixture::client_ = nullptr;
          
          BT::NodeConfiguration * CancelDriveOnHeadingTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          CancelDriveOnHeadingTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> CancelDriveOnHeadingTestFixture::tree_ = nullptr;
          
          TEST_F(  CancelDriveOnHeadingTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <CancelDriveOnHeading name="CancelDriveOnHeading"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           auto send_goal_options = rclcpp_action::Client
           <nav2_msgs::action::DriveOnHeading>::SendGoalOptions(   );
          
           // Creating a dummy goal_msg
           auto goal_msg = nav2_msgs::action::DriveOnHeading::Goal(   );
          
           // Setting target pose
           goal_msg.target.x = 0.5;
          
           // DriveOnHeadingCancel for server and sending a goal
           client_->wait_for_action_server(   );
           client_->async_send_goal(  goal_msg,   send_goal_options );
          
           // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
          
           // Executing tick
           tree_->rootNode(   )->executeTick(   );
          
           // BT node should return success,   once when the goal is cancelled
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Adding another test case to check if the goal is infact cancelling
           EXPECT_EQ(  action_server_->isGoalCancelled(   ),   true );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and drive on new thread
           CancelDriveOnHeadingTestFixture::action_server_ = std::make_shared<CancelDriveOnHeadingServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  CancelDriveOnHeadingTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
          
      29  class FollowPathActionServer : public TestActionServer<nav2_msgs::action::FollowPath>
          {
          public:
      32   FollowPathActionServer(   )
           : TestActionServer(  "follow_path" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::FollowPath::Result>(   );
           goal_handle->succeed(  result );
           }
          };
          
      48  class FollowPathActionTestFixture : public ::testing::Test
          {
          public:
      51   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "follow_path_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::FollowPathAction>(  
           name,   "follow_path",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::FollowPathAction>(  
           "FollowPath",   builder );
           }
          
      83   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<FollowPathActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr FollowPathActionTestFixture::node_ = nullptr;
          std::shared_ptr<FollowPathActionServer>
          FollowPathActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * FollowPathActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> FollowPathActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> FollowPathActionTestFixture::tree_ = nullptr;
          
          TEST_F(  FollowPathActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <FollowPath path="{path}" controller_id="FollowPath"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // set new path on blackboard
           nav_msgs::msg::Path path;
           path.poses.resize(  1 );
           path.poses[0].pose.position.x = 1.0;
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // the goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<std::string>(  "controller_id" ),   std::string(  "FollowPath" ) );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path.poses.size(   ),   1u );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path.poses[0].pose.position.x,   1.0 );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->controller_id,   std::string(  "FollowPath" ) );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal
           path.poses[0].pose.position.x = -2.5;
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   path );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path.poses.size(   ),   1u );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path.poses[0].pose.position.x,   -2.5 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           FollowPathActionTestFixture::action_server_ =
           std::make_shared<FollowPathActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  FollowPathActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <memory>
          #include <set>
          #include <string>
          
          #include "../../test_action_server.hpp"
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "std_msgs/msg/string.hpp"
          
      28  class GoalCheckerSelectorTestFixture : public ::testing::Test
          {
          public:
      31   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "goal_checker_selector_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
          
           BT::NodeBuilder builder = [](  const std::string & name,   const BT::NodeConfiguration & config ) {
           return std::make_unique<nav2_behavior_tree::GoalCheckerSelector>(  name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::GoalCheckerSelector>(  
           "GoalCheckerSelector",  
           builder );
           }
          
      52   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> GoalCheckerSelectorTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> GoalCheckerSelectorTestFixture::tree_ = nullptr;
          
          TEST_F(  GoalCheckerSelectorTestFixture,   test_custom_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="SimpleGoalCheck" topic_name="goal_checker_selector_custom_topic_name"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_goal_checker_result;
           config_->blackboard->get(  "selected_goal_checker",   selected_goal_checker_result );
          
           EXPECT_EQ(  selected_goal_checker_result,   "SimpleGoalCheck" );
          
           std_msgs::msg::String selected_goal_checker_cmd;
          
           selected_goal_checker_cmd.data = "AngularGoalChecker";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto goal_checker_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "goal_checker_selector_custom_topic_name",   qos );
          
           // publish a few updates of the selected_goal_checker
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           goal_checker_selector_pub->publish(  selected_goal_checker_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check goal_checker updated
           config_->blackboard->get(  "selected_goal_checker",   selected_goal_checker_result );
           EXPECT_EQ(  "AngularGoalChecker",   selected_goal_checker_result );
          }
          
          TEST_F(  GoalCheckerSelectorTestFixture,   test_default_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_goal_checker_result;
           config_->blackboard->get(  "selected_goal_checker",   selected_goal_checker_result );
          
           EXPECT_EQ(  selected_goal_checker_result,   "GridBased" );
          
           std_msgs::msg::String selected_goal_checker_cmd;
          
           selected_goal_checker_cmd.data = "RRT";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto goal_checker_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "goal_checker_selector",   qos );
          
           // publish a few updates of the selected_goal_checker
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           goal_checker_selector_pub->publish(  selected_goal_checker_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check goal_checker updated
           config_->blackboard->get(  "selected_goal_checker",   selected_goal_checker_result );
           EXPECT_EQ(  "RRT",   selected_goal_checker_result );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
          
      31  class NavigateThroughPosesActionServer
      32   : public TestActionServer<nav2_msgs::action::NavigateThroughPoses>
          {
          public:
      35   NavigateThroughPosesActionServer(   )
           : TestActionServer(  "navigate_through_poses" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateThroughPoses>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::NavigateThroughPoses::Result>(   );
           goal_handle->succeed(  result );
           }
          };
          
      51  class NavigateThroughPosesActionTestFixture : public ::testing::Test
          {
          public:
      54   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "navigate_through_poses_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           std::vector<geometry_msgs::msg::PoseStamped> poses;
           config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(  
           "goals",   poses );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(  
           name,   "navigate_through_poses",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(  
           "NavigateThroughPoses",   builder );
           }
          
      89   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<NavigateThroughPosesActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr NavigateThroughPosesActionTestFixture::node_ = nullptr;
          std::shared_ptr<NavigateThroughPosesActionServer>
          NavigateThroughPosesActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * NavigateThroughPosesActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> NavigateThroughPosesActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> NavigateThroughPosesActionTestFixture::tree_ = nullptr;
          
          TEST_F(  NavigateThroughPosesActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <NavigateThroughPoses goals="{goals}" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           std::vector<geometry_msgs::msg::PoseStamped> poses;
           poses.resize(  1 );
           poses[0].pose.position.x = -2.5;
           poses[0].pose.orientation.x = 1.0;
           config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   poses );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->poses,   poses );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           NavigateThroughPosesActionTestFixture::action_server_ =
           std::make_shared<NavigateThroughPosesActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  NavigateThroughPosesActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
          
      30  class NavigateToPoseActionServer : public TestActionServer<nav2_msgs::action::NavigateToPose>
          {
          public:
      33   NavigateToPoseActionServer(   )
           : TestActionServer(  "navigate_to_pose" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateToPose>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::NavigateToPose::Result>(   );
           goal_handle->succeed(  result );
           }
          };
          
      49  class NavigateToPoseActionTestFixture : public ::testing::Test
          {
          public:
      52   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "navigate_to_pose_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(  
           name,   "navigate_to_pose",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(  
           "NavigateToPose",   builder );
           }
          
      84   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<NavigateToPoseActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr NavigateToPoseActionTestFixture::node_ = nullptr;
          std::shared_ptr<NavigateToPoseActionServer>
          NavigateToPoseActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * NavigateToPoseActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> NavigateToPoseActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> NavigateToPoseActionTestFixture::tree_ = nullptr;
          
          TEST_F(  NavigateToPoseActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <NavigateToPose goal="{goal}" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           geometry_msgs::msg::PoseStamped pose;
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->pose,   pose );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal
           pose.pose.position.x = -2.5;
           pose.pose.orientation.x = 1.0;
           config_->blackboard->set<geometry_msgs::msg::PoseStamped>(  "goal",   pose );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->pose,   pose );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           NavigateToPoseActionTestFixture::action_server_ =
           std::make_shared<NavigateToPoseActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  NavigateToPoseActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Pablo Iñigo Blasco
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <memory>
          #include <set>
          #include <string>
          
          #include "../../test_action_server.hpp"
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "std_msgs/msg/string.hpp"
          
      28  class PlannerSelectorTestFixture : public ::testing::Test
          {
          public:
      31   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "planner_selector_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
          
           BT::NodeBuilder builder = [](  const std::string & name,   const BT::NodeConfiguration & config ) {
           return std::make_unique<nav2_behavior_tree::PlannerSelector>(  name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::PlannerSelector>(  "PlannerSelector",   builder );
           }
          
      50   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr PlannerSelectorTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> PlannerSelectorTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> PlannerSelectorTestFixture::tree_ = nullptr;
          
          TEST_F(  PlannerSelectorTestFixture,   test_custom_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector_custom_topic_name"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_planner_result;
           config_->blackboard->get(  "selected_planner",   selected_planner_result );
          
           EXPECT_EQ(  selected_planner_result,   "GridBased" );
          
           std_msgs::msg::String selected_planner_cmd;
          
           selected_planner_cmd.data = "RRT";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto planner_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "planner_selector_custom_topic_name",   qos );
          
           // publish a few updates of the selected_planner
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           planner_selector_pub->publish(  selected_planner_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check planner updated
           config_->blackboard->get(  "selected_planner",   selected_planner_result );
           EXPECT_EQ(  "RRT",   selected_planner_result );
          }
          
          TEST_F(  PlannerSelectorTestFixture,   test_default_topic )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check default value
           std::string selected_planner_result;
           config_->blackboard->get(  "selected_planner",   selected_planner_result );
          
           EXPECT_EQ(  selected_planner_result,   "GridBased" );
          
           std_msgs::msg::String selected_planner_cmd;
          
           selected_planner_cmd.data = "RRT";
          
           rclcpp::QoS qos(  rclcpp::KeepLast(  1 ) );
           qos.transient_local(   ).reliable(   );
          
           auto planner_selector_pub =
           node_->create_publisher<std_msgs::msg::String>(  "planner_selector",   qos );
          
           // publish a few updates of the selected_planner
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           planner_selector_pub->publish(  selected_planner_cmd );
          
           rclcpp::spin_some(  node_ );
           }
          
           // check planner updated
           config_->blackboard->get(  "selected_planner",   selected_planner_result );
           EXPECT_EQ(  "RRT",   selected_planner_result );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_service.hpp"
          #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
          
      26  class ReinitializeGlobalLocalizationService : public TestService<std_srvs::srv::Empty>
          {
          public:
      29   ReinitializeGlobalLocalizationService(   )
           : TestService(  "reinitialize_global_localization" )
           {}
          };
          
      34  class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test
          {
          public:
      37   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "reinitialize_global_localization_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           factory_->registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(  
           "ReinitializeGlobalLocalization" );
           }
          
      62   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<ReinitializeGlobalLocalizationService> server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr ReinitializeGlobalLocalizationServiceTestFixture::node_ = nullptr;
          std::shared_ptr<ReinitializeGlobalLocalizationService>
          ReinitializeGlobalLocalizationServiceTestFixture::server_ = nullptr;
          BT::NodeConfiguration * ReinitializeGlobalLocalizationServiceTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          ReinitializeGlobalLocalizationServiceTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> ReinitializeGlobalLocalizationServiceTestFixture::tree_ = nullptr;
          
          TEST_F(  ReinitializeGlobalLocalizationServiceTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <ReinitializeGlobalLocalization service_name="reinitialize_global_localization"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->executeTick(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize service and spin on new thread
           ReinitializeGlobalLocalizationServiceTestFixture::server_ =
           std::make_shared<ReinitializeGlobalLocalizationService>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  ReinitializeGlobalLocalizationServiceTestFixture::server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          #include <vector>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
          #include "../../test_behavior_tree_fixture.hpp"
          
      32  class RemovePassedGoalsTestFixture : public ::testing::Test
          {
          public:
      35   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "passed_goals_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
           transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(  node_ );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(  
           "tf_buffer",  
           transform_handler_->getBuffer(   ) );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::RemovePassedGoals>(  
           name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::RemovePassedGoals>(  
           "RemovePassedGoals",   builder );
           }
          
      64   static void TearDownTestCase(   )
           {
           transform_handler_->deactivate(   );
           delete config_;
           config_ = nullptr;
           transform_handler_.reset(   );
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
           static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
          };
          
          rclcpp::Node::SharedPtr RemovePassedGoalsTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * RemovePassedGoalsTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> RemovePassedGoalsTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> RemovePassedGoalsTestFixture::tree_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::TransformHandler>
          RemovePassedGoalsTestFixture::transform_handler_ = nullptr;
          
          TEST_F(  RemovePassedGoalsTestFixture,   test_tick )
          {
           geometry_msgs::msg::Pose pose;
           pose.position.x = 0.25;
           pose.position.y = 0.0;
          
           transform_handler_->activate(   );
           transform_handler_->waitForTransform(   );
           transform_handler_->updateRobotPose(  pose );
          
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           std::vector<geometry_msgs::msg::PoseStamped> poses;
           poses.resize(  4 );
           poses[0].pose.position.x = 0.0;
           poses[0].pose.position.y = 0.0;
          
           poses[1].pose.position.x = 0.5;
           poses[1].pose.position.y = 0.0;
          
           poses[2].pose.position.x = 1.0;
           poses[2].pose.position.y = 0.0;
          
           poses[3].pose.position.x = 2.0;
           poses[3].pose.position.y = 0.0;
          
           config_->blackboard->set(  "goals",   poses );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // check that it removed the point in range
           std::vector<geometry_msgs::msg::PoseStamped> output_poses;
           config_->blackboard->get(  "goals",   output_poses );
          
           EXPECT_EQ(  output_poses.size(   ),   2u );
           EXPECT_EQ(  output_poses[0],   poses[2] );
           EXPECT_EQ(  output_poses[1],   poses[3] );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
          
      29  class SmoothPathActionServer : public TestActionServer<nav2_msgs::action::SmoothPath>
          {
          public:
      32   SmoothPathActionServer(   )
           : TestActionServer(  "smooth_path" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<
           rclcpp_action::ServerGoalHandle<nav2_msgs::action::SmoothPath>> goal_handle )
           override
           {
           const auto goal = goal_handle->get_goal(   );
           auto result = std::make_shared<nav2_msgs::action::SmoothPath::Result>(   );
           goal_handle->succeed(  result );
           }
          };
          
      48  class SmoothPathActionTestFixture : public ::testing::Test
          {
          public:
      51   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "smooth_path_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SmoothPathAction>(  
           name,   "smooth_path",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::SmoothPathAction>(  
           "SmoothPath",   builder );
           }
          
      83   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<SmoothPathActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr SmoothPathActionTestFixture::node_ = nullptr;
          std::shared_ptr<SmoothPathActionServer>
          SmoothPathActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * SmoothPathActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> SmoothPathActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> SmoothPathActionTestFixture::tree_ = nullptr;
          
          TEST_F(  SmoothPathActionTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <SmoothPath unsmoothed_path="{unsmoothed_path}" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           nav_msgs::msg::Path path;
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           // goal should have reached our server
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path,   path );
          
           // halt node so another goal can be sent
           tree_->rootNode(   )->halt(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::IDLE );
          
           // set new goal
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = -2.5;
           pose.pose.orientation.x = 1.0;
           path.poses.push_back(  pose );
           config_->blackboard->set<nav_msgs::msg::Path>(  "unsmoothed_path",   path );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           nav_msgs::msg::Path path_empty;
           EXPECT_NE(  path_empty,   path );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->path,   path );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           SmoothPathActionTestFixture::action_server_ =
           std::make_shared<SmoothPathActionServer>(   );
          
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  SmoothPathActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/spin_action.hpp"
          
      26  class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>
          {
          public:
      29   SpinActionServer(   )
           : TestActionServer(  "spin" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
           goal_handle )
           override
           {
           nav2_msgs::action::Spin::Result::SharedPtr result =
           std::make_shared<nav2_msgs::action::Spin::Result>(   );
           bool return_success = getReturnSuccess(   );
           if (  return_success ) {
           goal_handle->succeed(  result );
           } else {
           goal_handle->abort(  result );
           }
           }
          };
          
          class SpinActionTestFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "spin_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SpinAction>(  
           name,   "spin",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::SpinAction>(  "Spin",   builder );
           }
          
           static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<SpinActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr SpinActionTestFixture::node_ = nullptr;
          std::shared_ptr<SpinActionServer> SpinActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * SpinActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> SpinActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> SpinActionTestFixture::tree_ = nullptr;
          
          TEST_F(  SpinActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Spin server_name="spin"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "spin_dist" ),   1.57 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Spin spin_dist="3.14" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<double>(  "spin_dist" ),   3.14 );
          }
          
          TEST_F(  SpinActionTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Spin spin_dist="3.14" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->target_yaw,   3.14f );
          }
          
          TEST_F(  SpinActionTestFixture,   test_failure )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Spin spin_dist="3.14" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           action_server_->setReturnSuccess(  false );
          
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
          
           std::cout << tree_->rootNode(   )->status(   );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
           EXPECT_EQ(  action_server_->getCurrentGoal(   )->target_yaw,   3.14f );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           SpinActionTestFixture::action_server_ = std::make_shared<SpinActionServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  SpinActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
          #include "lifecycle_msgs/srv/change_state.hpp"
          
      26  class CancelSpinServer : public TestActionServer<nav2_msgs::action::Spin>
          {
          public:
      29   CancelSpinServer(   )
           : TestActionServer(  "spin" )
           {}
          
          protected:
      34   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
           goal_handle )
           {
           while (  !goal_handle->is_canceling(   ) ) {
           // Spining here until goal cancels
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
           }
           }
          };
          
      45  class CancelSpinActionTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "cancel_spin_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           client_ = rclcpp_action::create_client<nav2_msgs::action::Spin>(  
           node_,   "spin" );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::SpinCancel>(  
           name,   "spin",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::SpinCancel>(  "CancelSpin",   builder );
           }
          
      80   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           client_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<CancelSpinServer> action_server_;
      96   static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>> client_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr CancelSpinActionTestFixture::node_ = nullptr;
          std::shared_ptr<CancelSpinServer>
          CancelSpinActionTestFixture::action_server_ = nullptr;
          std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>>
          CancelSpinActionTestFixture::client_ = nullptr;
          
          BT::NodeConfiguration * CancelSpinActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          CancelSpinActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> CancelSpinActionTestFixture::tree_ = nullptr;
          
          TEST_F(  CancelSpinActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <CancelSpin name="SpinCancel"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Spin>::SendGoalOptions(   );
          
           // Creating a dummy goal_msg
           auto goal_msg = nav2_msgs::action::Spin::Goal(   );
          
           // Setting target yaw
           goal_msg.target_yaw = 1.57;
          
           // Spining for server and sending a goal
           client_->wait_for_action_server(   );
           client_->async_send_goal(  goal_msg,   send_goal_options );
          
           // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
          
           // Executing tick
           tree_->rootNode(   )->executeTick(   );
          
           // BT node should return success,   once when the goal is cancelled
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Adding another test case to check if the goal is infact cancelling
           EXPECT_EQ(  action_server_->isGoalCancelled(   ),   true );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           CancelSpinActionTestFixture::action_server_ = std::make_shared<CancelSpinServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  CancelSpinActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
          
          
      31  class TruncatePathTestFixture : public ::testing::Test
          {
          public:
      34   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "change_goal_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::TruncatePath>(  
           name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::TruncatePath>(  
           "TruncatePath",   builder );
           }
          
      59   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr TruncatePathTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * TruncatePathTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> TruncatePathTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> TruncatePathTestFixture::tree_ = nullptr;
          
          TEST_F(  TruncatePathTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           nav_msgs::msg::Path path;
           path.header.stamp = node_->now(   );
          
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = 0;
           pose.pose.position.y = 0;
           pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  0.0 );
           path.poses.push_back(  pose );
          
           pose.pose.position.x = 0.5;
           pose.pose.position.y = 0.0;
           path.poses.push_back(  pose );
          
           pose.pose.position.x = 0.9;
           pose.pose.position.y = 0.0;
           path.poses.push_back(  pose );
          
           pose.pose.position.x = 1.5;
           pose.pose.position.y = 0.5;
           path.poses.push_back(  pose );
          
           EXPECT_EQ(  path.poses.size(   ),   4u );
          
           config_->blackboard->set(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           nav_msgs::msg::Path truncated_path;
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_NE(  path,   truncated_path );
           EXPECT_EQ(  truncated_path.poses.size(   ),   2u );
          
           double r,   p,   y;
           tf2::Quaternion q;
           tf2::fromMsg(  truncated_path.poses.back(   ).pose.orientation,   q );
           tf2::Matrix3x3(  q ).getRPY(  r,   p,   y );
          
           EXPECT_NEAR(  y,   0.463,   0.001 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp

          // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
          
          
      30  class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
           void SetUp(   ) override
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::TruncatePathLocal>(  
           "truncate_path_local",   *config_ );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::TruncatePathLocal>(  
           name,   config );
           };
           try {
           factory_->registerBuilder<nav2_behavior_tree::TruncatePathLocal>(  
           "TruncatePathLocal",   builder );
           } catch (  BT::BehaviorTreeException const & ) {
           // ignoring multiple registrations of TruncatePathLocal
           }
           }
          
           void TearDown(   ) override
           {
           bt_node_.reset(   );
           tree_.reset(   );
           }
          
           static geometry_msgs::msg::PoseStamped poseMsg(  double x,   double y,   double orientation )
           {
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = x;
           pose.pose.position.y = y;
           pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  orientation );
           return pose;
           }
          
           nav_msgs::msg::Path createLoopCrossingTestPath(   )
           {
           nav_msgs::msg::Path path;
           path.header.stamp = node_->now(   );
           path.header.frame_id = "map";
          
           // this is a loop to make it harder for robot to find the proper closest pose
           path.poses.push_back(  poseMsg(  -0.3,   -1.2,   -M_PI * 3 / 2 ) );
           // the position is closest to robot but orientation is different
           path.poses.push_back(  poseMsg(  -0.3,   0.0,   -M_PI * 3 / 2 ) );
           path.poses.push_back(  poseMsg(  -0.5,   1.0,   -M_PI ) );
           path.poses.push_back(  poseMsg(  -1.5,   1.0,   -M_PI / 2 ) );
           path.poses.push_back(  poseMsg(  -1.5,   0.0,   0.0 ) );
          
           // this is the correct path section for the first match
           path.poses.push_back(  poseMsg(  -0.5,   0.0,   0.0 ) );
           path.poses.push_back(  poseMsg(  0.4,   0.0,   0.0 ) );
           path.poses.push_back(  poseMsg(  1.5,   0.0,   0.0 ) );
          
           // this is a loop to make it harder for robot to find the proper closest pose
           path.poses.push_back(  poseMsg(  1.5,   1.0,   M_PI / 2 ) );
           path.poses.push_back(  poseMsg(  0.5,   1.0,   M_PI ) );
           // the position is closest to robot but orientation is different
           path.poses.push_back(  poseMsg(  0.3,   0.0,   M_PI * 3 / 2 ) );
           path.poses.push_back(  poseMsg(  0.3,   -1.0,   M_PI * 3 / 2 ) );
          
           return path;
           }
          
          protected:
           static std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> bt_node_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> TruncatePathLocalTestFixture::bt_node_ =
           nullptr;
          std::shared_ptr<BT::Tree> TruncatePathLocalTestFixture::tree_ = nullptr;
          
          TEST_F(  TruncatePathLocalTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePathLocal
           distance_forward="2.0"
           distance_backward="1.0"
           robot_frame="base_link"
           transform_tolerance="0.2"
           angular_distance_weight="0.2"
           pose="{pose}"
           input_path="{path}"
           output_path="{truncated_path}"
           />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create path and set it on blackboard
           nav_msgs::msg::Path path = createLoopCrossingTestPath(   );
           EXPECT_EQ(  path.poses.size(   ),   12u );
          
           config_->blackboard->set(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
          
           nav_msgs::msg::Path truncated_path;
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   3u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   -0.5 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   1.5 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   0.0 );
          
           /////////////////////////////////////////
           // should match the first loop crossing
           config_->blackboard->set(  "pose",   poseMsg(  0.0,   0.0,   M_PI / 2 ) );
          
           tree_->haltTree(   );
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   2u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   -0.3 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   -0.5 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   1.0 );
          
           /////////////////////////////////////////
           // should match the last loop crossing
           config_->blackboard->set(  "pose",   poseMsg(  0.0,   0.0,   -M_PI / 2 ) );
          
           tree_->haltTree(   );
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   2u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   0.3 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   0.3 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   -1.0 );
          
           SUCCEED(   );
          }
          
          TEST_F(  TruncatePathLocalTestFixture,   test_success_on_empty_path )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePathLocal
           distance_forward="2.0"
           distance_backward="1.0"
           robot_frame="base_link"
           transform_tolerance="0.2"
           angular_distance_weight="0.2"
           pose="{pose}"
           input_path="{path}"
           output_path="{truncated_path}"
           max_robot_pose_search_dist="infinity"
           />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create path and set it on blackboard
           nav_msgs::msg::Path path;
           path.header.stamp = node_->now(   );
           path.header.frame_id = "map";
          
           config_->blackboard->set(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           nav_msgs::msg::Path truncated_path;
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  path,   truncated_path );
           SUCCEED(   );
          }
          
          TEST_F(  TruncatePathLocalTestFixture,   test_failure_on_no_pose )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePathLocal
           distance_forward="2.0"
           distance_backward="1.0"
           transform_tolerance="0.2"
           angular_distance_weight="0.2"
           robot_frame="{robot_frame}"
           input_path="{path}"
           output_path="{truncated_path}"
           max_robot_pose_search_dist="infinity"
           />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create path and set it on blackboard
           nav_msgs::msg::Path path;
           path.header.stamp = node_->now(   );
           path.header.frame_id = "map";
          
           config_->blackboard->set(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           nav_msgs::msg::Path truncated_path;
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
           SUCCEED(   );
          }
          
          TEST_F(  TruncatePathLocalTestFixture,   test_failure_on_invalid_robot_frame )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePathLocal
           distance_forward="2.0"
           distance_backward="1.0"
           transform_tolerance="0.2"
           robot_frame="invalid_frame"
           angular_distance_weight="0.2"
           input_path="{path}"
           output_path="{truncated_path}"
           max_robot_pose_search_dist="infinity"
           />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           nav_msgs::msg::Path path = createLoopCrossingTestPath(   );
           EXPECT_EQ(  path.poses.size(   ),   12u );
          
           config_->blackboard->set(  "path",   path );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           nav_msgs::msg::Path truncated_path;
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
           SUCCEED(   );
          }
          
          TEST_F(  TruncatePathLocalTestFixture,   test_path_pruning )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TruncatePathLocal
           distance_forward="2.0"
           distance_backward="1.0"
           robot_frame="base_link"
           transform_tolerance="0.2"
           angular_distance_weight="0.0"
           pose="{pose}"
           input_path="{path}"
           output_path="{truncated_path}"
           max_robot_pose_search_dist="3.0"
           />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create path and set it on blackboard
           nav_msgs::msg::Path path = createLoopCrossingTestPath(   );
           nav_msgs::msg::Path truncated_path;
          
           config_->blackboard->set(  "path",   path );
          
           /////////////////////////////////////////
           // should match the first loop crossing
           config_->blackboard->set(  "pose",   poseMsg(  0.0,   0.0,   0.0 ) );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   2u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   -0.3 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   -0.5 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   1.0 );
          
           /////////////////////////////////////////
           // move along the path to leave the first loop crossing behind
           config_->blackboard->set(  "pose",   poseMsg(  -1.5,   1.0,   0.0 ) );
           // tick until node succeeds
           tree_->haltTree(   );
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           // this truncated_path is not interesting,   let's proceed to the second loop crossing
          
           /////////////////////////////////////////
           // should match the second loop crossing
           config_->blackboard->set(  "pose",   poseMsg(  0.0,   0.0,   0.0 ) );
           // tick until node succeeds
           tree_->haltTree(   );
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   3u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   -0.5 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   1.5 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   0.0 );
          
           /////////////////////////////////////////
           // move along the path to leave the second loop crossing behind
           config_->blackboard->set(  "pose",   poseMsg(  1.5,   1.0,   0.0 ) );
           // tick until node succeeds
           tree_->haltTree(   );
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           // this truncated_path is not interesting,   let's proceed to the last loop crossing
          
           /////////////////////////////////////////
           // should match the last loop crossing
           config_->blackboard->set(  "pose",   poseMsg(  0.0,   0.0,   0.0 ) );
           // tick until node succeeds
           tree_->haltTree(   );
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS &&
           tree_->rootNode(   )->status(   ) != BT::NodeStatus::FAILURE )
           {
           tree_->rootNode(   )->executeTick(   );
           }
           config_->blackboard->get(  "truncated_path",   truncated_path );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_NE(  path,   truncated_path );
           ASSERT_GE(  truncated_path.poses.size(   ),   1u );
           EXPECT_EQ(  truncated_path.poses.size(   ),   2u );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.x,   0.3 );
           EXPECT_EQ(  truncated_path.poses.front(   ).pose.position.y,   0.0 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.x,   0.3 );
           EXPECT_EQ(  truncated_path.poses.back(   ).pose.position.y,   -1.0 );
          
           SUCCEED(   );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/wait_action.hpp"
          
      26  class WaitActionServer : public TestActionServer<nav2_msgs::action::Wait>
          {
          public:
      29   WaitActionServer(   )
           : TestActionServer(  "wait" )
           {}
          
          protected:
           void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>> )
           override
           {}
          };
          
      40  class WaitActionTestFixture : public ::testing::Test
          {
          public:
      43   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "wait_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           config_->blackboard->set<int>(  "number_recoveries",   0 );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::WaitAction>(  
           name,   "wait",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::WaitAction>(  "Wait",   builder );
           }
          
      75   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           factory_.reset(   );
           }
          
           void SetUp(   ) override
           {
           config_->blackboard->set(  "number_recoveries",   0 );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<WaitActionServer> action_server_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr WaitActionTestFixture::node_ = nullptr;
          std::shared_ptr<WaitActionServer> WaitActionTestFixture::action_server_ = nullptr;
          BT::NodeConfiguration * WaitActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> WaitActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> WaitActionTestFixture::tree_ = nullptr;
          
          TEST_F(  WaitActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Wait />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<int>(  "wait_duration" ),   1 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Wait wait_duration="10" />
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  tree_->rootNode(   )->getInput<int>(  "wait_duration" ),   10 );
          }
          
          TEST_F(  WaitActionTestFixture,   test_tick )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <Wait wait_duration="-5"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   0 );
          
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  config_->blackboard->get<int>(  "number_recoveries" ),   1 );
           EXPECT_EQ(  rclcpp::Duration(  action_server_->getCurrentGoal(   )->time ).seconds(   ),   5.0 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           WaitActionTestFixture::action_server_ = std::make_shared<WaitActionServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  WaitActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
          #include "lifecycle_msgs/srv/change_state.hpp"
          
      26  class CancelWaitServer : public TestActionServer<nav2_msgs::action::Wait>
          {
          public:
      29   CancelWaitServer(   )
           : TestActionServer(  "wait" )
           {}
          
          protected:
      34   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>>
           goal_handle )
           {
           while (  !goal_handle->is_canceling(   ) ) {
           // waiting here until goal cancels
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
           }
           }
          };
          
      45  class CancelWaitActionTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "cancel_wait_action_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           client_ = rclcpp_action::create_client<nav2_msgs::action::Wait>(  
           node_,   "wait" );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::WaitCancel>(  
           name,   "wait",   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::WaitCancel>(  "CancelWait",   builder );
           }
          
      80   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           action_server_.reset(   );
           client_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
           static std::shared_ptr<CancelWaitServer> action_server_;
      96   static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>> client_;
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr CancelWaitActionTestFixture::node_ = nullptr;
          std::shared_ptr<CancelWaitServer>
          CancelWaitActionTestFixture::action_server_ = nullptr;
          std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>>
          CancelWaitActionTestFixture::client_ = nullptr;
          
          BT::NodeConfiguration * CancelWaitActionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          CancelWaitActionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> CancelWaitActionTestFixture::tree_ = nullptr;
          
          TEST_F(  CancelWaitActionTestFixture,   test_ports )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <CancelWait name="WaitCancel"/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Wait>::SendGoalOptions(   );
          
           // Creating a dummy goal_msg
           auto goal_msg = nav2_msgs::action::Wait::Goal(   );
          
           // Setting a waiting time for 5 Seconds.
           goal_msg.time.sec = 5;
          
           // Waiting for server and sending a goal
           client_->wait_for_action_server(   );
           client_->async_send_goal(  goal_msg,   send_goal_options );
          
           // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
           std::this_thread::sleep_for(  std::chrono::milliseconds(  15 ) );
          
           // Executing tick
           tree_->rootNode(   )->executeTick(   );
          
           // BT node should return success,   once when the goal is cancelled
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Adding another test case to check if the goal is infact cancelling
           EXPECT_EQ(  action_server_->isGoalCancelled(   ),   true );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize action server and spin on new thread
           CancelWaitActionTestFixture::action_server_ = std::make_shared<CancelWaitServer>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  CancelWaitActionTestFixture::action_server_ );
           } );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
          
      25  class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      28   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::DistanceTraveledCondition>(  
           "distance_traveled",   *config_ );
           }
          
      34   void TearDown(   )
           {
           bt_node_.reset(   );
           }
          
          protected:
      40   static std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition> bt_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition>
          DistanceTraveledConditionTestFixture::bt_node_ = nullptr;
          
      46  TEST_F(  DistanceTraveledConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = 0;
           pose.pose.position.y = 0;
           pose.pose.orientation.w = 1;
          
           double traveled = 0;
           for (  int i = 1; i <= 20; i++ ) {
           pose.pose.position.x = i * 0.51;
           transform_handler_->updateRobotPose(  pose.pose );
          
           // Wait for transforms to actually update
           // updated pose is i * 0.51
           // we wait for the traveled distance to reach a value > i * 0.5
           // we can assume the current transform has been updated at this point
           while (  traveled < i * 0.5 ) {
           if (  nav2_util::getCurrentPose(  pose,   *transform_handler_->getBuffer(   ) ) ) {
           traveled = pose.pose.position.x;
           }
           }
          
           if (  i % 2 ) {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           } else {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           }
           }
          }
          
      79  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp

       1  // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
          
      25  class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      28   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::GloballyUpdatedGoalCondition>(  
           "globally_updated_goal",   *config_ );
           }
          
      34   void TearDown(   )
           {
           bt_node_.reset(   );
           }
          
          protected:
      40   static std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition> bt_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition>
          GloballyUpdatedGoalConditionTestFixture::bt_node_ = nullptr;
          
      46  TEST_F(  GloballyUpdatedGoalConditionTestFixture,   test_behavior )
          {
           geometry_msgs::msg::PoseStamped goal;
           config_->blackboard->set(  "goal",   goal );
          
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           goal.pose.position.x = 1.0;
           config_->blackboard->set(  "goal",   goal );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          }
          
      60  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      30  class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      33   void SetUp(   )
           {
           node_->declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue{0.1} );
          
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           goal.header.frame_id = "map";
           goal.pose.position.x = 1.0;
           goal.pose.position.y = 1.0;
           config_->blackboard->set(  "goal",   goal );
          
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <GoalReached goal="{goal}" />
           </BehaviorTree>
           </root> )";
      51  
           factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>(  "GoalReached" );
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           }
          
           void TearDown(   )
      57   {
           tree_.reset(   );
           }
          
          protected:
      62   static std::shared_ptr<BT::Tree> tree_;
          };
          
          std::shared_ptr<BT::Tree> GoalReachedConditionTestFixture::tree_ = nullptr;
          
          TEST_F(  GoalReachedConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           geometry_msgs::msg::Pose pose;
           pose.position.x = 0.0;
           pose.position.y = 0.0;
           transform_handler_->updateRobotPose(  pose );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           pose.position.x = 0.5;
           pose.position.y = 0.5;
           transform_handler_->updateRobotPose(  pose );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           pose.position.x = 0.9;
           pose.position.y = 0.9;
           transform_handler_->updateRobotPose(  pose );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::SUCCESS );
          
           pose.position.x = 1.0;
           pose.position.y = 1.0;
      92   transform_handler_->updateRobotPose(  pose );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
          
      26  class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      29   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedCondition>(  
           "goal_updated",   *config_ );
           }
          
      35   void TearDown(   )
           {
           bt_node_.reset(   );
           }
          
          protected:
      41   static std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition> bt_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition>
          GoalUpdatedConditionTestFixture::bt_node_ = nullptr;
          
      47  TEST_F(  GoalUpdatedConditionTestFixture,   test_behavior )
          {
           geometry_msgs::msg::PoseStamped goal;
           config_->blackboard->set(  "goal",   goal );
          
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           goal.pose.position.x = 1.0;
           config_->blackboard->set(  "goal",   goal );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          }
          
      61  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
          
      24  class TestNode : public BT::SyncActionNode
          {
          public:
      27   TestNode(  const std::string & name,   const BT::NodeConfiguration & config )
           : SyncActionNode(  name,   config )
           {}
          
      31   BT::NodeStatus tick(   )
           {
           return BT::NodeStatus::SUCCESS;
           }
          
      36   static BT::PortsList providedPorts(   )
           {
           return {};
           }
          };
          
      42  class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      45   void SetUp(   )
           {
           test_node_ = std::make_shared<TestNode>(  "TestNode",   *config_ );
           }
          
      50   void TearDown(   )
           {
           test_node_.reset(   );
           }
          
          protected:
      56   static std::shared_ptr<TestNode> test_node_;
          };
          
          std::shared_ptr<TestNode> InitialPoseReceivedConditionTestFixture::test_node_ = nullptr;
          
      61  TEST_F(  InitialPoseReceivedConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  nav2_behavior_tree::initialPoseReceived(  *test_node_ ),   BT::NodeStatus::FAILURE );
           config_->blackboard->set(  "initial_pose_received",   true );
           EXPECT_EQ(  nav2_behavior_tree::initialPoseReceived(  *test_node_ ),   BT::NodeStatus::SUCCESS );
          }
          
      68  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          #include <chrono>
          
          #include "sensor_msgs/msg/battery_state.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
          
      27  class IsBatteryLowConditionTestFixture : public ::testing::Test
          {
          public:
      30   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "test_is_battery_low" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
          
           factory_->registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>(  "IsBatteryLow" );
          
           battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(  
           "/battery_status",  
           rclcpp::SystemDefaultsQoS(   ) );
           }
          
      51   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           battery_pub_.reset(   );
           node_.reset(   );
           factory_.reset(   );
           }
          
          protected:
      61   static rclcpp::Node::SharedPtr node_;
      62   static BT::NodeConfiguration * config_;
      63   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
      64   static rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
          };
          
          rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr;
          BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryLowConditionTestFixture::factory_ = nullptr;
          rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
          IsBatteryLowConditionTestFixture::battery_pub_ = nullptr;
          
      73  TEST_F(  IsBatteryLowConditionTestFixture,   test_behavior_percentage )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <IsBatteryLow min_battery="0.5" battery_topic="/battery_status"/>
           </BehaviorTree>
           </root> )";
          
           auto tree = factory_->createTreeFromText(  xml_txt,   config_->blackboard );
          
           sensor_msgs::msg::BatteryState battery_msg;
           battery_msg.percentage = 1.0;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           battery_msg.percentage = 0.49;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::SUCCESS );
          
           battery_msg.percentage = 0.51;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           battery_msg.percentage = 0.0;
           battery_pub_->publish(  battery_msg );
     106   std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::SUCCESS );
          }
          
          TEST_F(  IsBatteryLowConditionTestFixture,   test_behavior_voltage )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <IsBatteryLow min_battery="5.0" battery_topic="/battery_status" is_voltage="true"/>
           </BehaviorTree>
           </root> )";
          
           auto tree = factory_->createTreeFromText(  xml_txt,   config_->blackboard );
          
           sensor_msgs::msg::BatteryState battery_msg;
           battery_msg.voltage = 10.0;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           battery_msg.voltage = 4.9;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::SUCCESS );
          
           battery_msg.voltage = 5.1;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
     139   rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::FAILURE );
          
           battery_msg.voltage = 0.0;
           battery_pub_->publish(  battery_msg );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           rclcpp::spin_some(  node_ );
           EXPECT_EQ(  tree.tickRoot(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp

       1  // Copyright (  c ) 2021 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "../../test_service.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      32  class IsPathValidService : public TestService<nav2_msgs::srv::IsPathValid>
          {
          public:
      35   IsPathValidService(   )
           : TestService(  "is_path_valid" )
           {}
          
      39   virtual void handle_service(  
      40   const std::shared_ptr<rmw_request_id_t> request_header,  
      41   const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,  
      42   const std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response )
           {
           (  void )request_header;
           (  void )request;
           response->is_valid = true;
           }
          };
          
      50  class IsPathValidTestFixture : public ::testing::Test
          {
          public:
      53   void SetUp(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "test_is_path_valid_condition" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
           config_ = new BT::NodeConfiguration(   );
           config_->blackboard = BT::Blackboard::create(   );
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  10 ) );
           factory_->registerNodeType<nav2_behavior_tree::IsPathValidCondition>(  "IsPathValid" );
           }
          
      66   void TearDown(   )
           {
           delete config_;
           config_ = nullptr;
           server_.reset(   );
           node_.reset(   );
           factory_.reset(   );
           tree_.reset(   );
           }
          
      76   static std::shared_ptr<IsPathValidService> server_;
          
          protected:
      79   static rclcpp::Node::SharedPtr node_;
      80   static BT::NodeConfiguration * config_;
      81   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
      82   static std::shared_ptr<BT::Tree> tree_;
          };
          
          std::shared_ptr<IsPathValidService> IsPathValidTestFixture::server_ = nullptr;
          rclcpp::Node::SharedPtr IsPathValidTestFixture::node_ = nullptr;
          BT::NodeConfiguration * IsPathValidTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> IsPathValidTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> IsPathValidTestFixture::tree_ = nullptr;
          
      91  TEST_F(  IsPathValidTestFixture,   test_behavior )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <IsPathValid/>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
     102   std::this_thread::sleep_for(  500ms );
          
           EXPECT_EQ(  tree_->rootNode(   )->executeTick(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           // initialize service and spin on new thread
           IsPathValidTestFixture::server_ = std::make_shared<IsPathValidService>(   );
           std::thread server_thread(  [](   ) {
           rclcpp::spin(  IsPathValidTestFixture::server_ );
           } );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           server_thread.join(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_stuck.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      31  class IsStuckTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      34   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::IsStuckCondition>(  "is_stuck",   *config_ );
           }
          
      39   void TearDown(   )
           {
           bt_node_.reset(   );
           }
          
          protected:
      45   static std::shared_ptr<nav2_behavior_tree::IsStuckCondition> bt_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::IsStuckCondition>
          IsStuckTestFixture::bt_node_ = nullptr;
          
      51  TEST_F(  IsStuckTestFixture,   test_behavior )
          {
           auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>(  "odom",   1 );
           nav_msgs::msg::Odometry odom_msg;
          
           // fill up odometry history with zero velocity
           auto time = node_->now(   );
           odom_msg.header.stamp = time;
           odom_msg.twist.twist.linear.x = 0.0;
           odom_pub->publish(  odom_msg );
           std::this_thread::sleep_for(  500ms );
           odom_pub->publish(  odom_msg );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           // huge negative velocity to simulate sudden brake
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.1 );
           odom_msg.twist.twist.linear.x = -1.5;
           odom_pub->publish(  odom_msg );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          
           // huge positive velocity means robot is not stuck anymore
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.2 );
           odom_msg.twist.twist.linear.x = 1.0;
           odom_pub->publish(  odom_msg );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           // stuck again due to negative velocity change is smaller time period
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.25 );
           odom_msg.twist.twist.linear.x = 0.0;
           odom_pub->publish(  odom_msg );
           std::this_thread::sleep_for(  500ms );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          }
          
      88  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp

       1  // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      29  class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      32   void SetUp(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "test_path_expiring_condition" );
           config_ = new BT::NodeConfiguration(   );
           config_->blackboard = BT::Blackboard::create(   );
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ );
           bt_node_ = std::make_shared<nav2_behavior_tree::PathExpiringTimerCondition>(  
           "time_expired",   *config_ );
           }
          
      42   void TearDown(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      51   static rclcpp::Node::SharedPtr node_;
      52   static std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition> bt_node_;
      53   static BT::NodeConfiguration * config_;
          };
          
          rclcpp::Node::SharedPtr PathExpiringTimerConditionTestFixture::node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition>
          PathExpiringTimerConditionTestFixture::bt_node_ = nullptr;
          BT::NodeConfiguration * PathExpiringTimerConditionTestFixture::config_ = nullptr;
          
      61  TEST_F(  PathExpiringTimerConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           for (  int i = 0; i < 20; ++i ) {
           rclcpp::sleep_for(  500ms );
           if (  i % 2 ) {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           } else {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           }
           }
          
           // place a new path on the blackboard to reset the timer
           nav_msgs::msg::Path path;
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = 1.0;
           path.poses.push_back(  pose );
          
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   path );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           rclcpp::sleep_for(  1500ms );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          }
          
      87  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      30  class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      33   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::TimeExpiredCondition>(  
           "time_expired",   *config_ );
           }
          
      39   void TearDown(   )
           {
           bt_node_.reset(   );
           }
          
          protected:
      45   static std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition> bt_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition>
          TimeExpiredConditionTestFixture::bt_node_ = nullptr;
          
      51  TEST_F(  TimeExpiredConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           for (  int i = 0; i < 20; ++i ) {
           rclcpp::sleep_for(  500ms );
           if (  i % 2 ) {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           } else {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           }
           }
          }
          
      66  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <chrono>
          #include <string>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
          
      25  class TransformAvailableConditionTestFixture : public ::testing::Test
          {
          public:
      28   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "test_behavior_tree_fixture" );
           transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(  node_ );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(  
           "tf_buffer",  
           transform_handler_->getBuffer(   ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
           }
          
      54   static void TearDownTestCase(   )
           {
           transform_handler_->deactivate(   );
           delete config_;
           config_ = nullptr;
           transform_handler_.reset(   );
           node_.reset(   );
           factory_.reset(   );
           }
          
      64   void SetUp(   )
           {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <TransformAvailable child="base_link" parent="map" />
           </BehaviorTree>
           </root> )";
          
      74   factory_->registerNodeType<nav2_behavior_tree::TransformAvailableCondition>(  
           "TransformAvailable" );
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
           }
          
           void TearDown(   )
      80   {
      81   tree_.reset(   );
      82   }
      83  
      84  protected:
           static rclcpp::Node::SharedPtr node_;
           static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr TransformAvailableConditionTestFixture::node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::TransformHandler>
          TransformAvailableConditionTestFixture::transform_handler_ = nullptr;
      95  BT::NodeConfiguration * TransformAvailableConditionTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory>
          TransformAvailableConditionTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> TransformAvailableConditionTestFixture::tree_ = nullptr;
          
          TEST_F(  TransformAvailableConditionTestFixture,   test_behavior )
          {
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::FAILURE );
     103   transform_handler_->activate(   );
           transform_handler_->waitForTransform(   );
           EXPECT_EQ(  tree_->tickRoot(   ),   BT::NodeStatus::SUCCESS );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
          
      23  class PipelineSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
           void SetUp(   ) override
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::PipelineSequence>(  
           "pipeline_sequence",   *config_ );
           first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->addChild(  first_child_.get(   ) );
           bt_node_->addChild(  second_child_.get(   ) );
           bt_node_->addChild(  third_child_.get(   ) );
           }
          
           void TearDown(   ) override
           {
           first_child_.reset(   );
      41   second_child_.reset(   );
      42   third_child_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
           static std::shared_ptr<nav2_behavior_tree::PipelineSequence> bt_node_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
          };
          
          std::shared_ptr<nav2_behavior_tree::PipelineSequence>
          PipelineSequenceTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          PipelineSequenceTestFixture::first_child_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          PipelineSequenceTestFixture::second_child_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          PipelineSequenceTestFixture::third_child_ = nullptr;
          
          TEST_F(  PipelineSequenceTestFixture,   test_failure_on_idle_child )
          {
           first_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_THROW(  bt_node_->executeTick(   ),   std::runtime_error );
          }
          
          TEST_F(  PipelineSequenceTestFixture,   test_failure )
          {
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          
           first_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           second_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          
           first_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          
           first_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          TEST_F(  PipelineSequenceTestFixture,   test_behavior )
          {
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           second_child_->changeStatus(  BT::NodeStatus::IDLE );
           third_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           second_child_->changeStatus(  BT::NodeStatus::RUNNING );
           third_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
          
          // Changes status to SUCCESS after a specified number of failures
      24  class RecoveryDummy : public nav2_behavior_tree::DummyNode
          {
          public:
           BT::NodeStatus tick(   ) override
           {
           if (  ticks_ == num_success_ ) {
           setStatus(  BT::NodeStatus::SUCCESS );
           } else if (  ticks_ == num_failure_ ) {
           setStatus(  BT::NodeStatus::FAILURE );
           }
           ticks_++;
           return status(   );
           }
          
           void returnSuccessOn(  int tick )
           {
           num_success_ = tick;
           ticks_ = 0;
           }
          
           void returnFailureOn(  int tick )
           {
           num_failure_ = tick;
           ticks_ = 0;
           }
          
          private:
           int ticks_{0};
           int num_success_{-1};
           int num_failure_{-1};
          };
          
          class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
           void SetUp(   ) override
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::RecoveryNode>(  
           "recovery_node",   *config_ );
           first_child_ = std::make_shared<RecoveryDummy>(   );
           second_child_ = std::make_shared<RecoveryDummy>(   );
           bt_node_->addChild(  first_child_.get(   ) );
           bt_node_->addChild(  second_child_.get(   ) );
           }
          
           void TearDown(   ) override
           {
           first_child_.reset(   );
           second_child_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
           static std::shared_ptr<nav2_behavior_tree::RecoveryNode> bt_node_;
           static std::shared_ptr<RecoveryDummy> first_child_;
           static std::shared_ptr<RecoveryDummy> second_child_;
          };
          
          std::shared_ptr<nav2_behavior_tree::RecoveryNode> RecoveryNodeTestFixture::bt_node_ = nullptr;
          std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::first_child_ = nullptr;
          std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::second_child_ = nullptr;
          
          TEST_F(  RecoveryNodeTestFixture,   test_only_two_children )
          {
           nav2_behavior_tree::DummyNode dummy;
           bt_node_->addChild(  &dummy );
           EXPECT_THROW(  bt_node_->executeTick(   ),   BT::BehaviorTreeException );
          }
          
          TEST_F(  RecoveryNodeTestFixture,   test_running )
          {
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          }
          
          TEST_F(  RecoveryNodeTestFixture,   test_failure_on_idle_child )
          {
           first_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_THROW(  bt_node_->executeTick(   ),   BT::LogicError );
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_THROW(  bt_node_->executeTick(   ),   BT::LogicError );
          }
          
          TEST_F(  RecoveryNodeTestFixture,   test_success_one_retry )
          {
           // first child returns success right away
           first_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          
           // first child fails,   second child succeeds,   then first child succeeds (  one retry )
           first_child_->returnSuccessOn(  1 );
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          TEST_F(  RecoveryNodeTestFixture,   test_failure_one_retry )
          {
           // first child fails,   second child fails
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
          
           // first child fails,   second child succeeds,   then first child fails (  one retry )
           first_child_->returnFailureOn(  1 );
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
          
      23  class RoundRobinNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
           void SetUp(   ) override
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::RoundRobinNode>(  
           "round_robin",   *config_ );
           first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->addChild(  first_child_.get(   ) );
           bt_node_->addChild(  second_child_.get(   ) );
           bt_node_->addChild(  third_child_.get(   ) );
           }
          
           void TearDown(   ) override
           {
           first_child_.reset(   );
      41   second_child_.reset(   );
      42   third_child_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
           static std::shared_ptr<nav2_behavior_tree::RoundRobinNode> bt_node_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
           static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
          };
          
          std::shared_ptr<nav2_behavior_tree::RoundRobinNode> RoundRobinNodeTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::first_child_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::second_child_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::third_child_ = nullptr;
          
          TEST_F(  RoundRobinNodeTestFixture,   test_failure_on_idle_child )
          {
           first_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_THROW(  bt_node_->executeTick(   ),   BT::LogicError );
          }
          
          TEST_F(  RoundRobinNodeTestFixture,   test_failure )
          {
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           second_child_->changeStatus(  BT::NodeStatus::FAILURE );
           third_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           third_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          TEST_F(  RoundRobinNodeTestFixture,   test_behavior )
          {
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           second_child_->changeStatus(  BT::NodeStatus::IDLE );
           third_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::RUNNING );
           third_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          
           first_child_->changeStatus(  BT::NodeStatus::IDLE );
           second_child_->changeStatus(  BT::NodeStatus::IDLE );
           third_child_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::RUNNING );
           second_child_->changeStatus(  BT::NodeStatus::IDLE );
           third_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           first_child_->changeStatus(  BT::NodeStatus::FAILURE );
           second_child_->changeStatus(  BT::NodeStatus::SUCCESS );
           third_child_->changeStatus(  BT::NodeStatus::FAILURE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  first_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  second_child_->status(   ),   BT::NodeStatus::IDLE );
           EXPECT_EQ(  third_child_->status(   ),   BT::NodeStatus::IDLE );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <cmath>
          #include <memory>
          #include <set>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
          
      28  class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      31   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::DistanceController>(  
           "distance_controller",   *config_ );
           dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->setChild(  dummy_node_.get(   ) );
           }
          
      39   void TearDown(   )
           {
           dummy_node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      46   static std::shared_ptr<nav2_behavior_tree::DistanceController> bt_node_;
      47   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::DistanceController>
          DistanceControllerTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          DistanceControllerTestFixture::dummy_node_ = nullptr;
          
      55  TEST_F(  DistanceControllerTestFixture,   test_behavior )
          {
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
          
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = 0;
           pose.pose.position.y = 0;
           pose.pose.orientation.w = 1;
          
           double traveled = 0;
           for (  int i = 1; i <= 20; i++ ) {
           pose.pose.position.x = i * 0.51;
           transform_handler_->updateRobotPose(  pose.pose );
          
           // Wait for transforms to actually update
           // updated pose is i * 0.55
           // we wait fot the traveled distance to reach a value > i * 0.5
           // we can assume the current transform has been updated at this point
           while (  traveled < i * 0.5 ) {
           if (  nav2_util::getCurrentPose(  pose,   *transform_handler_->getBuffer(   ) ) ) {
           traveled = std::sqrt(  
           pose.pose.position.x * pose.pose.position.x +
           pose.pose.position.y * pose.pose.position.y );
           }
           }
          
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
          
           if (  i % 2 ) {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           } else {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
           }
           }
          }
          
      96  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      27  class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      30   void SetUp(   )
           {
           // Setting fake goals on blackboard
           geometry_msgs::msg::PoseStamped goal1;
           goal1.header.stamp = node_->now(   );
           std::vector<geometry_msgs::msg::PoseStamped> poses1;
           poses1.push_back(  goal1 );
           config_->blackboard->set(  "goal",   goal1 );
           config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   poses1 );
          
           bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(  
           "goal_updated_controller",   *config_ );
           dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->setChild(  dummy_node_.get(   ) );
           }
          
      46   void TearDown(   )
           {
           dummy_node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      53   static std::shared_ptr<nav2_behavior_tree::GoalUpdatedController> bt_node_;
      54   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::GoalUpdatedController>
          GoalUpdatedControllerTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          GoalUpdatedControllerTestFixture::dummy_node_ = nullptr;
          
      62  TEST_F(  GoalUpdatedControllerTestFixture,   test_behavior )
          {
           // Creating updated fake-goals
           geometry_msgs::msg::PoseStamped goal2;
           goal2.header.stamp = node_->now(   );
           std::vector<geometry_msgs::msg::PoseStamped> poses2;
           poses2.push_back(  goal2 );
          
           // starting in idle
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick for the first time,   dummy node should be ticked
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick again with updated goal,   dummy node should be ticked
           config_->blackboard->set(  "goal",   goal2 );
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick again without update,   dummy node should not be ticked
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick again with updated goals,   dummy node should be ticked
           config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   poses2 );
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          }
          
      95  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Francisco Martin Rico
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          
          #include "../../test_action_server.hpp"
          #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
          
          
      30  class GoalUpdaterTestFixture : public ::testing::Test
          {
          public:
      33   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "goal_updater_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::GoalUpdater>(  
           name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::GoalUpdater>(  
           "GoalUpdater",   builder );
           }
          
      58   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr GoalUpdaterTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * GoalUpdaterTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> GoalUpdaterTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> GoalUpdaterTestFixture::tree_ = nullptr;
          
          TEST_F(  GoalUpdaterTestFixture,   test_tick )
          {
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
           <AlwaysSuccess/>
           </GoalUpdater>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // create new goal and set it on blackboard
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           goal.pose.position.x = 1.0;
           config_->blackboard->set(  "goal",   goal );
          
           // tick until node succeeds
           while (  tree_->rootNode(   )->status(   ) != BT::NodeStatus::SUCCESS ) {
           tree_->rootNode(   )->executeTick(   );
           }
          
           geometry_msgs::msg::PoseStamped updated_goal;
           config_->blackboard->get(  "updated_goal",   updated_goal );
          
           EXPECT_EQ(  updated_goal,   goal );
          
           geometry_msgs::msg::PoseStamped goal_to_update;
           goal_to_update.header.stamp = node_->now(   );
           goal_to_update.pose.position.x = 2.0;
          
           auto goal_updater_pub =
           node_->create_publisher<geometry_msgs::msg::PoseStamped>(  "goal_update",   10 );
          
           auto start = node_->now(   );
           while (  (  node_->now(   ) - start ).seconds(   ) < 0.5 ) {
           tree_->rootNode(   )->executeTick(   );
           goal_updater_pub->publish(  goal_to_update );
          
           rclcpp::spin_some(  node_ );
           }
          
           config_->blackboard->get(  "updated_goal",   updated_goal );
          
           EXPECT_NE(  updated_goal,   goal );
           EXPECT_EQ(  updated_goal,   goal_to_update );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp

          // Copyright (  c ) 2022 Neobotix GmbH
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav_msgs/msg/path.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      32  class PathLongerOnApproachTestFixture : public ::testing::Test
          {
          public:
      35   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "path_longer_on_approach_test_fixture" );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
          
           BT::NodeBuilder builder =
           [](  const std::string & name,   const BT::NodeConfiguration & config )
           {
           return std::make_unique<nav2_behavior_tree::PathLongerOnApproach>(  
           name,   config );
           };
          
           factory_->registerBuilder<nav2_behavior_tree::PathLongerOnApproach>(  
           "PathLongerOnApproach",   builder );
           }
          
      60   static void TearDownTestCase(   )
           {
           delete config_;
           config_ = nullptr;
           node_.reset(   );
           factory_.reset(   );
           }
          
           void TearDown(   ) override
           {
           tree_.reset(   );
           }
          
          protected:
           static rclcpp::Node::SharedPtr node_;
           static BT::NodeConfiguration * config_;
           static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
           static std::shared_ptr<BT::Tree> tree_;
          };
          
          rclcpp::Node::SharedPtr PathLongerOnApproachTestFixture::node_ = nullptr;
          
          BT::NodeConfiguration * PathLongerOnApproachTestFixture::config_ = nullptr;
          std::shared_ptr<BT::BehaviorTreeFactory> PathLongerOnApproachTestFixture::factory_ = nullptr;
          std::shared_ptr<BT::Tree> PathLongerOnApproachTestFixture::tree_ = nullptr;
          
          TEST_F(  PathLongerOnApproachTestFixture,   test_tick )
          {
           // Success test
           // create tree
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PathLongerOnApproach path="{path}" prox_len="5.0" length_factor="2.0">
           <AlwaysSuccess/>
           </PathLongerOnApproach>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // set new path on blackboard
           nav_msgs::msg::Path new_path;
           new_path.poses.resize(  10 );
           for (  unsigned int i = 0; i < new_path.poses.size(   ); i++ ) {
           // Assuming distance between waypoints to be 1.5m
           new_path.poses[i].pose.position.x = 1.5 * i;
           }
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   new_path );
          
           tree_->rootNode(   )->executeTick(   );
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::SUCCESS );
          
           // Failure test
           // create tree
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PathLongerOnApproach path="{path}" prox_len="20.0" length_factor="1.0">
           <AlwaysFailure/>
           </PathLongerOnApproach>
           </BehaviorTree>
           </root> )";
          
           tree_ = std::make_shared<BT::Tree>(  factory_->createTreeFromText(  xml_txt,   config_->blackboard ) );
          
           // set old path on blackboard
           nav_msgs::msg::Path old_path;
           old_path.poses.resize(  5 );
           for (  unsigned int i = 1; i <= old_path.poses.size(   ); i++ ) {
           // Assuming distance between waypoints to be 3.0m
           old_path.poses[i - 1].pose.position.x = 3.0 * i;
           }
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   old_path );
           tree_->rootNode(   )->executeTick(   );
          
           // set new path on blackboard
           new_path.poses.resize(  11 );
           for (  unsigned int i = 0; i <= new_path.poses.size(   ); i++ ) {
           // Assuming distance between waypoints to be 1.5m
           new_path.poses[i].pose.position.x = 1.5 * i;
           }
           config_->blackboard->set<nav_msgs::msg::Path>(  "path",   new_path );
           tree_->rootNode(   )->executeTick(   );
          
           EXPECT_EQ(  tree_->rootNode(   )->status(   ),   BT::NodeStatus::FAILURE );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           int all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      27  class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      30   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::RateController>(  
           "rate_controller",   *config_ );
           dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->setChild(  dummy_node_.get(   ) );
           }
          
      38   void TearDown(   )
           {
           dummy_node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      45   static std::shared_ptr<nav2_behavior_tree::RateController> bt_node_;
      46   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::RateController>
          RateControllerTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          RateControllerTestFixture::dummy_node_ = nullptr;
          
      54  TEST_F(  RateControllerTestFixture,   test_behavior )
          {
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
          
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           for (  int i = 0; i < 10; ++i ) {
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           std::this_thread::sleep_for(  500ms );
           if (  i % 2 ) {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
           } else {
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           }
           }
          }
          
      74  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      27  class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      30   void SetUp(   )
           {
           bt_node_ = std::make_shared<nav2_behavior_tree::SingleTrigger>(  
           "single_trigger",   *config_ );
           dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->setChild(  dummy_node_.get(   ) );
           }
          
      38   void TearDown(   )
           {
           dummy_node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      45   static std::shared_ptr<nav2_behavior_tree::SingleTrigger> bt_node_;
      46   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::SingleTrigger>
          SingleTriggerTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          SingleTriggerTestFixture::dummy_node_ = nullptr;
          
      54  TEST_F(  SingleTriggerTestFixture,   test_behavior )
          {
           // starting in idle
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick once,   should work
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // tick again with dummy node success,   should fail
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           // tick again with dummy node idle,   should still fail
           dummy_node_->changeStatus(  BT::NodeStatus::IDLE );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          
           // halt BT for a new execution run,   should work when dummy node is running
           // and once when dummy node returns success and then fail
           bt_node_->halt(   );
           dummy_node_->changeStatus(  BT::NodeStatus::RUNNING );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::FAILURE );
          }
          
      81  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <chrono>
          #include <memory>
          #include <set>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          #include "../../test_behavior_tree_fixture.hpp"
          #include "../../test_dummy_tree_node.hpp"
          #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      32  class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
          {
          public:
      35   void SetUp(   )
           {
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           config_->blackboard->set(  "goal",   goal );
          
           std::vector<geometry_msgs::msg::PoseStamped> fake_poses;
           config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(  "goals",   fake_poses ); // NOLINT
          
           bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>(  "speed_controller",   *config_ );
           dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>(   );
           bt_node_->setChild(  dummy_node_.get(   ) );
           }
          
      49   void TearDown(   )
           {
           dummy_node_.reset(   );
           bt_node_.reset(   );
           }
          
          protected:
      56   static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
      57   static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
          };
          
          std::shared_ptr<nav2_behavior_tree::SpeedController>
          SpeedControllerTestFixture::bt_node_ = nullptr;
          std::shared_ptr<nav2_behavior_tree::DummyNode>
          SpeedControllerTestFixture::dummy_node_ = nullptr;
          
          /*
           * Test for speed controller behavior
           * Speed controller calculates the period after which it should succeed
           * based on the current velocity which is scaled to a pre-defined rate range
           * Current velocity is set using odom messages
           * The period is reset on the basis of current velocity after the last period is exceeded
           */
      72  TEST_F(  SpeedControllerTestFixture,   test_behavior )
          {
           auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>(  "odom",   1 );
           nav_msgs::msg::Odometry odom_msg;
          
           auto time = node_->now(   );
           odom_msg.header.stamp = time;
           odom_msg.twist.twist.linear.x = 0.223;
           odom_pub->publish(  odom_msg );
          
           EXPECT_EQ(  bt_node_->status(   ),   BT::NodeStatus::IDLE );
          
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
           EXPECT_EQ(  dummy_node_->status(   ),   BT::NodeStatus::IDLE );
          
           // after the first tick,   period should be a default value of 1s
           // first tick should return running since period has not exceeded
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           // set the child node to success so node can return success
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
          
           // should return success since period has exceeded and new period should be set to ~2s
           rclcpp::sleep_for(  1s );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          
           // send new velocity for update after the next period
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.5 );
           odom_msg.twist.twist.linear.x = 0;
           odom_msg.twist.twist.linear.y = 0;
           odom_pub->publish(  odom_msg );
          
           // Period should be set to ~2s based on the last speed of 0.223 m/s
           rclcpp::sleep_for(  1s );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
          
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
           rclcpp::sleep_for(  1s );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          
           // period should be set to ~10s based on the last speed of 0 m/s
           // should return running for the first 9 seconds
           for (  int i = 0; i < 9; ++i ) {
           rclcpp::sleep_for(  1s );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::RUNNING );
           }
          
           // set the child node to success so node can return success
           dummy_node_->changeStatus(  BT::NodeStatus::SUCCESS );
          
           // should return success since period has exceeded
           rclcpp::sleep_for(  1s );
           EXPECT_EQ(  bt_node_->executeTick(   ),   BT::NodeStatus::SUCCESS );
          }
          
     128  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_behavior_tree/test/test_action_server.hpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_ACTION_SERVER_HPP_
          #define TEST_ACTION_SERVER_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          
          template<class ActionT>
      25  class TestActionServer : public rclcpp::Node
          {
          public:
      28   explicit TestActionServer(  
           std::string action_name,  
           const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) )
           : Node(  "test_action_server",   options )
           {
           using namespace std::placeholders; // NOLINT
          
           this->action_server_ = rclcpp_action::create_server<ActionT>(  
           this->get_node_base_interface(   ),  
           this->get_node_clock_interface(   ),  
           this->get_node_logging_interface(   ),  
           this->get_node_waitables_interface(   ),  
           action_name,  
           std::bind(  &TestActionServer::handle_goal,   this,   _1,   _2 ),  
           std::bind(  &TestActionServer::handle_cancel,   this,   _1 ),  
           std::bind(  &TestActionServer::handle_accepted,   this,   _1 ) );
           }
          
      46   std::shared_ptr<const typename ActionT::Goal> getCurrentGoal(   ) const
           {
           return current_goal_;
           }
          
      51   void setReturnSuccess(  bool return_success )
           {
           return_success_ = return_success;
           }
          
      56   bool getReturnSuccess(  void )
           {
           return return_success_;
           }
          
      61   bool isGoalCancelled(   )
           {
           return goal_cancelled_;
           }
          
          protected:
      67   virtual rclcpp_action::GoalResponse handle_goal(  
      68   const rclcpp_action::GoalUUID &,  
      69   std::shared_ptr<const typename ActionT::Goal> goal )
           {
           current_goal_ = goal;
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
           }
          
      75   virtual rclcpp_action::CancelResponse handle_cancel(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> )
           {
           goal_cancelled_ = true;
           return rclcpp_action::CancelResponse::ACCEPT;
           }
          
      82   virtual void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle ) = 0;
          
      85   void handle_accepted(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
           {
           using namespace std::placeholders; // NOLINT
           // this needs to return quickly to avoid blocking the executor,   so spin up a new thread
           std::thread{std::bind(  &TestActionServer::execute,   this,   _1 ),   goal_handle}.detach(   );
           }
          
          private:
           typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
           std::shared_ptr<const typename ActionT::Goal> current_goal_;
           bool return_success_ = true;
           bool goal_cancelled_ = false;
          };
          
          #endif // TEST_ACTION_SERVER_HPP_

navigation2/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_BEHAVIOR_TREE_FIXTURE_HPP_
          #define TEST_BEHAVIOR_TREE_FIXTURE_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <set>
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "rclcpp/rclcpp.hpp"
          
          #include "test_transform_handler.hpp"
          #include "test_dummy_tree_node.hpp"
          
          namespace nav2_behavior_tree
          {
          
      32  class BehaviorTreeTestFixture : public ::testing::Test
          {
          public:
      35   static void SetUpTestCase(   )
           {
           node_ = std::make_shared<rclcpp::Node>(  "test_behavior_tree_fixture" );
           transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(  node_ );
           factory_ = std::make_shared<BT::BehaviorTreeFactory>(   );
          
           config_ = new BT::NodeConfiguration(   );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           config_->blackboard = BT::Blackboard::create(   );
           // Put items on the blackboard
           config_->blackboard->set<rclcpp::Node::SharedPtr>(  
           "node",  
           node_ );
           config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(  
           "tf_buffer",  
           transform_handler_->getBuffer(   ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",  
           std::chrono::milliseconds(  20 ) );
           config_->blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",  
           std::chrono::milliseconds(  10 ) );
           config_->blackboard->set<bool>(  "initial_pose_received",   false );
          
           transform_handler_->activate(   );
           transform_handler_->waitForTransform(   );
           }
          
      64   static void TearDownTestCase(   )
           {
           transform_handler_->deactivate(   );
           delete config_;
           config_ = nullptr;
           transform_handler_.reset(   );
           node_.reset(   );
           factory_.reset(   );
           }
          
          protected:
      75   static rclcpp::Node::SharedPtr node_;
      76   static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
      77   static BT::NodeConfiguration * config_;
      78   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
          };
          
          } // namespace nav2_behavior_tree
          
          rclcpp::Node::SharedPtr nav2_behavior_tree::BehaviorTreeTestFixture::node_ = nullptr;
          
          std::shared_ptr<nav2_behavior_tree::TransformHandler>
          nav2_behavior_tree::BehaviorTreeTestFixture::transform_handler_ = nullptr;
          
          BT::NodeConfiguration * nav2_behavior_tree::BehaviorTreeTestFixture::config_ = nullptr;
          
          std::shared_ptr<BT::BehaviorTreeFactory>
          nav2_behavior_tree::BehaviorTreeTestFixture::factory_ = nullptr;
          
          #endif // TEST_BEHAVIOR_TREE_FIXTURE_HPP_

navigation2/nav2_behavior_tree/test/test_bt_conversions.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <chrono>
          #include <string>
          
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "nav2_behavior_tree/bt_conversions.hpp"
          
          template<typename T>
      29  class TestNode : public BT::SyncActionNode
          {
          public:
      32   TestNode(  const std::string & name,   const BT::NodeConfiguration & config )
           : SyncActionNode(  name,   config )
           {}
          
           BT::NodeStatus tick(   ) override
           {
           return BT::NodeStatus::SUCCESS;
           }
          
           static BT::PortsList providedPorts(   )
           {
           return {
           BT::InputPort<T>(  "test" )
           };
           }
          };
          
          
          TEST(  PointPortTest,   test_wrong_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PointPort test="1.0;2.0;3.0;4.0" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>(  "PointPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           geometry_msgs::msg::Point value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   0.0 );
           EXPECT_EQ(  value.y,   0.0 );
           EXPECT_EQ(  value.z,   0.0 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PointPort test="1.0;2.0" />
           </BehaviorTree>
           </root> )";
          
           tree = factory.createTreeFromText(  xml_txt );
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   0.0 );
           EXPECT_EQ(  value.y,   0.0 );
           EXPECT_EQ(  value.z,   0.0 );
          }
          
          TEST(  PointPortTest,   test_correct_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PointPort test="1.0;2.0;3.0" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>(  "PointPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           geometry_msgs::msg::Point value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   1.0 );
           EXPECT_EQ(  value.y,   2.0 );
           EXPECT_EQ(  value.z,   3.0 );
          }
          
          TEST(  QuaternionPortTest,   test_wrong_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <QuaternionPort test="1.0;2.0;3.0;4.0;5.0" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>(  "QuaternionPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           geometry_msgs::msg::Quaternion value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   0.0 );
           EXPECT_EQ(  value.y,   0.0 );
           EXPECT_EQ(  value.z,   0.0 );
           EXPECT_EQ(  value.w,   1.0 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <QuaternionPort test="1.0;2.0;3.0" />
           </BehaviorTree>
           </root> )";
          
           tree = factory.createTreeFromText(  xml_txt );
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   0.0 );
           EXPECT_EQ(  value.y,   0.0 );
           EXPECT_EQ(  value.z,   0.0 );
           EXPECT_EQ(  value.w,   1.0 );
          }
          
          TEST(  QuaternionPortTest,   test_correct_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <QuaternionPort test="0.7;0.0;0.0;0.7" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>(  "QuaternionPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           geometry_msgs::msg::Quaternion value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.x,   0.7 );
           EXPECT_EQ(  value.y,   0.0 );
           EXPECT_EQ(  value.z,   0.0 );
           EXPECT_EQ(  value.w,   0.7 );
          }
          
          TEST(  PoseStampedPortTest,   test_wrong_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>(  "PoseStampedPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           geometry_msgs::msg::PoseStamped value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  rclcpp::Time(  value.header.stamp ).nanoseconds(   ),   0 );
           EXPECT_EQ(  value.header.frame_id,   "" );
           EXPECT_EQ(  value.pose.position.x,   0.0 );
           EXPECT_EQ(  value.pose.position.y,   0.0 );
           EXPECT_EQ(  value.pose.position.z,   0.0 );
           EXPECT_EQ(  value.pose.orientation.x,   0.0 );
           EXPECT_EQ(  value.pose.orientation.y,   0.0 );
           EXPECT_EQ(  value.pose.orientation.z,   0.0 );
           EXPECT_EQ(  value.pose.orientation.w,   1.0 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
           </BehaviorTree>
           </root> )";
          
           tree = factory.createTreeFromText(  xml_txt );
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  rclcpp::Time(  value.header.stamp ).nanoseconds(   ),   0 );
           EXPECT_EQ(  value.header.frame_id,   "" );
           EXPECT_EQ(  value.pose.position.x,   0.0 );
           EXPECT_EQ(  value.pose.position.y,   0.0 );
           EXPECT_EQ(  value.pose.position.z,   0.0 );
           EXPECT_EQ(  value.pose.orientation.x,   0.0 );
           EXPECT_EQ(  value.pose.orientation.y,   0.0 );
           EXPECT_EQ(  value.pose.orientation.z,   0.0 );
           EXPECT_EQ(  value.pose.orientation.w,   1.0 );
          }
          
          TEST(  PoseStampedPortTest,   test_correct_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>(  "PoseStampedPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           tree = factory.createTreeFromText(  xml_txt );
           geometry_msgs::msg::PoseStamped value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  rclcpp::Time(  value.header.stamp ).nanoseconds(   ),   0 );
           EXPECT_EQ(  value.header.frame_id,   "map" );
           EXPECT_EQ(  value.pose.position.x,   1.0 );
           EXPECT_EQ(  value.pose.position.y,   2.0 );
           EXPECT_EQ(  value.pose.position.z,   3.0 );
           EXPECT_EQ(  value.pose.orientation.x,   4.0 );
           EXPECT_EQ(  value.pose.orientation.y,   5.0 );
           EXPECT_EQ(  value.pose.orientation.z,   6.0 );
           EXPECT_EQ(  value.pose.orientation.w,   7.0 );
          }
          
          TEST(  MillisecondsPortTest,   test_correct_syntax )
          {
           std::string xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <MillisecondsPort test="10000" />
           </BehaviorTree>
           </root> )";
          
           BT::BehaviorTreeFactory factory;
           factory.registerNodeType<TestNode<std::chrono::milliseconds>>(  "MillisecondsPort" );
           auto tree = factory.createTreeFromText(  xml_txt );
          
           std::chrono::milliseconds value;
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.count(   ),   10000 );
          
           xml_txt =
           R"(  
           <root main_tree_to_execute = "MainTree" >
           <BehaviorTree ID="MainTree">
           <MillisecondsPort test="123.4" />
           </BehaviorTree>
           </root> )";
          
           tree = factory.createTreeFromText(  xml_txt );
           tree.rootNode(   )->getInput(  "test",   value );
           EXPECT_EQ(  value.count(   ),   123 );
          }

navigation2/nav2_behavior_tree/test/test_dummy_tree_node.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_DUMMY_TREE_NODE_HPP_
          #define TEST_DUMMY_TREE_NODE_HPP_
          
          #include <behaviortree_cpp_v3/basic_types.h>
          #include <behaviortree_cpp_v3/action_node.h>
          
          namespace nav2_behavior_tree
          {
          
          /**
           * @brief A Dummy TreeNode to be used as a child for testing nodes
           * Returns the current status on tick without any execution logic
           */
      29  class DummyNode : public BT::ActionNodeBase
          {
          public:
      32   DummyNode(   )
           : BT::ActionNodeBase(  "dummy",   {} )
           {
           }
          
      37   void changeStatus(  BT::NodeStatus status )
           {
           setStatus(  status );
           }
          
           BT::NodeStatus executeTick(   ) override
           {
           return tick(   );
           }
          
      47   BT::NodeStatus tick(   ) override
           {
           return status(   );
           }
          
      52   void halt(   ) override
           {
           }
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // TEST_DUMMY_TREE_NODE_HPP_

navigation2/nav2_behavior_tree/test/test_service.hpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_SERVICE_HPP_
          #define TEST_SERVICE_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          
          template<class ServiceT>
      24  class TestService : public rclcpp::Node
          {
          public:
      27   explicit TestService(  
           std::string service_name,  
           const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) )
           : Node(  "test_service",   options )
           {
           using namespace std::placeholders; // NOLINT
          
           server_ = create_service<ServiceT>(  
           service_name,  
           std::bind(  &TestService::handle_service,   this,   _1,   _2,   _3 ) );
           }
          
      39   std::shared_ptr<typename ServiceT::Request> getCurrentRequest(   ) const
           {
           return current_request_;
           }
          
          protected:
      45   virtual void handle_service(  
      46   const std::shared_ptr<rmw_request_id_t> request_header,  
      47   const std::shared_ptr<typename ServiceT::Request> request,  
      48   const std::shared_ptr<typename ServiceT::Response> response )
           {
           (  void )request_header;
           (  void )response;
           current_request_ = request;
           }
          
          private:
           typename rclcpp::Service<ServiceT>::SharedPtr server_;
           std::shared_ptr<typename ServiceT::Request> current_request_;
          };
          
          #endif // TEST_SERVICE_HPP_

navigation2/nav2_behavior_tree/test/test_transform_handler.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_TRANSFORM_HANDLER_HPP_
          #define TEST_TRANSFORM_HANDLER_HPP_
          
          #include <memory>
          #include <string>
          #include <thread>
          #include <chrono>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/pose.hpp"
          #include "geometry_msgs/msg/transform_stamped.hpp"
          #include "tf2_msgs/msg/tf_message.hpp"
          #include "tf2_ros/transform_broadcaster.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/buffer.h"
          
          using namespace std::chrono_literals; // NOLINT
          using namespace std::chrono; // NOLINT
          
          namespace nav2_behavior_tree
          {
      40  class TransformHandler
          {
          public:
      43   explicit TransformHandler(  rclcpp::Node::SharedPtr & node )
           : node_(  node ),  
           is_active_(  false ),  
           base_transform_(  nullptr ),  
           tf_broadcaster_(  nullptr )
           {
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
           }
          
      53   ~TransformHandler(   )
           {
           if (  is_active_ ) {
           deactivate(   );
           }
           }
          
           // Activate the tester before running tests
      61   void activate(   )
           {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           }
           is_active_ = true;
          
           // Launch a thread to process the messages for this node
           spin_thread_ = std::make_unique<nav2_util::NodeThread>(  node_->get_node_base_interface(   ) );
          
           startRobotTransform(   );
           }
          
      74   void deactivate(   )
           {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
           is_active_ = false;
           spin_thread_.reset(   );
           tf_broadcaster_.reset(   );
           tf_buffer_.reset(   );
           tf_listener_.reset(   );
           }
          
      86   std::shared_ptr<tf2_ros::Buffer> getBuffer(   )
           {
           return tf_buffer_;
           }
          
      91   void waitForTransform(   )
           {
           if (  is_active_ ) {
           while (  !tf_buffer_->canTransform(  "map",   "base_link",   rclcpp::Time(  0 ) ) ) {
           std::this_thread::sleep_for(  100ms );
           }
           RCLCPP_INFO(  node_->get_logger(   ),   "Transforms are available now!" );
           return;
           }
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
          
     103   void updateRobotPose(  const geometry_msgs::msg::Pose & pose )
           {
           // Update base transform to publish
           base_transform_->transform.translation.x = pose.position.x;
           base_transform_->transform.translation.y = pose.position.y;
           base_transform_->transform.translation.z = pose.position.z;
           base_transform_->transform.rotation.x = pose.orientation.x;
           base_transform_->transform.rotation.y = pose.orientation.y;
           base_transform_->transform.rotation.z = pose.orientation.z;
           base_transform_->transform.rotation.w = pose.orientation.w;
           publishRobotTransform(   );
           }
          
          private:
     117   void publishRobotTransform(   )
           {
           base_transform_->header.stamp = node_->now(   );
           tf_broadcaster_->sendTransform(  *base_transform_ );
           }
          
     123   void startRobotTransform(   )
           {
           // Provide the robot pose transform
           tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(  node_ );
          
           if (  !base_transform_ ) {
           base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>(   );
           base_transform_->header.frame_id = "map";
           base_transform_->child_frame_id = "base_link";
           }
          
           // Set an initial pose
           geometry_msgs::msg::Pose robot_pose;
           robot_pose.position.x = 0;
           robot_pose.position.y = 0;
           robot_pose.orientation.w = 1;
           updateRobotPose(  robot_pose );
          
           // Publish the transform periodically
           transform_timer_ = node_->create_wall_timer(  
           100ms,   std::bind(  &TransformHandler::publishRobotTransform,   this ) );
           }
          
     146   rclcpp::Node::SharedPtr node_;
          
     148   bool is_active_;
          
           // A thread for spinning the ROS node
     151   std::unique_ptr<nav2_util::NodeThread> spin_thread_;
          
           // Subscriber
          
           // The tester must provide the robot pose through a transform
     156   std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
     157   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     158   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
     159   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
     160   rclcpp::TimerBase::SharedPtr transform_timer_;
          };
          
          } // namespace nav2_behavior_tree
          
          #endif // TEST_TRANSFORM_HANDLER_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp

       1  // Copyright (  c ) 2018 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          
          #include <chrono>
          #include <string>
          #include <memory>
          #include <vector>
          
          #include "nav2_util/lifecycle_node.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_core/behavior.hpp"
          
          #ifndef NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
          #define NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
          
          namespace behavior_server
          {
          
          /**
           * @class behavior_server::BehaviorServer
           * @brief An server hosting a map of behavior plugins
           */
      38  class BehaviorServer : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief A constructor for behavior_server::BehaviorServer
           * @param options Additional options to control creation of the node.
           */
      45   explicit BehaviorServer(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
      46   ~BehaviorServer(   );
          
           /**
           * @brief Loads behavior plugins from parameter file
           * @return bool if successfully loaded the plugins
           */
      52   bool loadBehaviorPlugins(   );
          
          protected:
           /**
           * @brief Configure lifecycle server
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Activate lifecycle server
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Deactivate lifecycle server
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Cleanup lifecycle server
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Shutdown lifecycle server
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
          
           // Plugins
           pluginlib::ClassLoader<nav2_core::Behavior> plugin_loader_;
           std::vector<pluginlib::UniquePtr<nav2_core::Behavior>> behaviors_;
           std::vector<std::string> default_ids_;
           std::vector<std::string> default_types_;
           std::vector<std::string> behavior_ids_;
           std::vector<std::string> behavior_types_;
          
           // Utilities
           std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
           std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
          };
          
          } // namespace behavior_server
          
          #endif // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
          #define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
          
          #include <memory>
          #include <string>
          #include <cmath>
          #include <chrono>
          #include <ctime>
          #include <thread>
          #include <utility>
          
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          #include "geometry_msgs/msg/twist.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_core/behavior.hpp"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          namespace nav2_behaviors
          {
          
      41  enum class Status : int8_t
          {
           SUCCEEDED = 1,  
           FAILED = 2,  
           RUNNING = 3,  
          };
          
          using namespace std::chrono_literals; //NOLINT
          
          /**
           * @class nav2_behaviors::Behavior
           * @brief An action server Behavior base class implementing the action server and basic factory.
           */
          template<typename ActionT>
      55  class TimedBehavior : public nav2_core::Behavior
          {
          public:
           using ActionServer = nav2_util::SimpleActionServer<ActionT>;
          
           /**
           * @brief A TimedBehavior constructor
           */
           TimedBehavior(   )
           : action_server_(  nullptr ),  
           cycle_frequency_(  10.0 ),  
           enabled_(  false ),  
           transform_tolerance_(  0.0 )
           {
           }
          
           virtual ~TimedBehavior(   ) = default;
          
           // Derived classes can override this method to catch the command and perform some checks
           // before getting into the main loop. The method will only be called
           // once and should return SUCCEEDED otherwise behavior will return FAILED.
      76   virtual Status onRun(  const std::shared_ptr<const typename ActionT::Goal> command ) = 0;
          
          
           // This is the method derived classes should mainly implement
           // and will be called cyclically while it returns RUNNING.
           // Implement the behavior such that it runs some unit of work on each call
           // and provides a status. The Behavior will finish once SUCCEEDED is returned
           // It's up to the derived class to define the final commanded velocity.
           virtual Status onCycleUpdate(   ) = 0;
          
           // an opportunity for derived classes to do something on configuration
           // if they chose
           virtual void onConfigure(   )
           {
           }
          
           // an opportunity for derived classes to do something on cleanup
           // if they chose
           virtual void onCleanup(   )
           {
           }
          
           // configure the server on lifecycle setup
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker ) override
           {
           node_ = parent;
           auto node = node_.lock(   );
          
           logger_ = node->get_logger(   );
          
           RCLCPP_INFO(  logger_,   "Configuring %s",   name.c_str(   ) );
          
           behavior_name_ = name;
           tf_ = tf;
          
           node->get_parameter(  "cycle_frequency",   cycle_frequency_ );
           node->get_parameter(  "global_frame",   global_frame_ );
           node->get_parameter(  "robot_base_frame",   robot_base_frame_ );
           node->get_parameter(  "transform_tolerance",   transform_tolerance_ );
          
           action_server_ = std::make_shared<ActionServer>(  
           node,   behavior_name_,  
           std::bind(  &TimedBehavior::execute,   this ) );
          
           collision_checker_ = collision_checker;
          
           vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>(  "cmd_vel",   1 );
          
           onConfigure(   );
           }
          
           // Cleanup server on lifecycle transition
           void cleanup(   ) override
           {
           action_server_.reset(   );
           vel_pub_.reset(   );
           onCleanup(   );
           }
          
           // Activate server on lifecycle transition
           void activate(   ) override
           {
           RCLCPP_INFO(  logger_,   "Activating %s",   behavior_name_.c_str(   ) );
          
           vel_pub_->on_activate(   );
           action_server_->activate(   );
           enabled_ = true;
           }
          
           // Deactivate server on lifecycle transition
           void deactivate(   ) override
           {
           vel_pub_->on_deactivate(   );
           action_server_->deactivate(   );
           enabled_ = false;
           }
          
          protected:
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
          
           std::string behavior_name_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
           std::shared_ptr<ActionServer> action_server_;
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           double cycle_frequency_;
           double enabled_;
           std::string global_frame_;
           std::string robot_base_frame_;
           double transform_tolerance_;
          
           // Clock
           rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
          
           // Logger
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_behaviors" )};
          
           // Main execution callbacks for the action server implementation calling the Behavior's
           // onRun and cycle functions to execute a specific behavior
           void execute(   )
           {
           RCLCPP_INFO(  logger_,   "Attempting %s",   behavior_name_.c_str(   ) );
          
           if (  !enabled_ ) {
           RCLCPP_WARN(  
           logger_,  
           "Called while inactive,   ignoring request." );
           return;
           }
          
           if (  onRun(  action_server_->get_current_goal(   ) ) != Status::SUCCEEDED ) {
           RCLCPP_INFO(  
           logger_,  
           "Initial checks failed for %s",   behavior_name_.c_str(   ) );
           action_server_->terminate_current(   );
           return;
           }
          
           auto start_time = steady_clock_.now(   );
          
           // Initialize the ActionT result
           auto result = std::make_shared<typename ActionT::Result>(   );
          
           rclcpp::WallRate loop_rate(  cycle_frequency_ );
          
           while (  rclcpp::ok(   ) ) {
           if (  action_server_->is_cancel_requested(   ) ) {
           RCLCPP_INFO(  logger_,   "Canceling %s",   behavior_name_.c_str(   ) );
           stopRobot(   );
           result->total_elapsed_time = steady_clock_.now(   ) - start_time;
           action_server_->terminate_all(  result );
           return;
           }
          
           // TODO(  orduno ) #868 Enable preempting a Behavior on-the-fly without stopping
           if (  action_server_->is_preempt_requested(   ) ) {
           RCLCPP_ERROR(  
           logger_,   "Received a preemption request for %s,  "
           " however feature is currently not implemented. Aborting and stopping.",  
           behavior_name_.c_str(   ) );
           stopRobot(   );
           result->total_elapsed_time = steady_clock_.now(   ) - start_time;
           action_server_->terminate_current(  result );
           return;
           }
          
           switch (  onCycleUpdate(   ) ) {
           case Status::SUCCEEDED:
           RCLCPP_INFO(  
           logger_,  
           "%s completed successfully",   behavior_name_.c_str(   ) );
           result->total_elapsed_time = steady_clock_.now(   ) - start_time;
           action_server_->succeeded_current(  result );
           return;
          
           case Status::FAILED:
           RCLCPP_WARN(  logger_,   "%s failed",   behavior_name_.c_str(   ) );
           result->total_elapsed_time = steady_clock_.now(   ) - start_time;
           action_server_->terminate_current(  result );
           return;
          
           case Status::RUNNING:
          
           default:
           loop_rate.sleep(   );
           break;
           }
           }
           }
          
           // Stop the robot with a commanded velocity
           void stopRobot(   )
           {
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
           cmd_vel->linear.x = 0.0;
           cmd_vel->linear.y = 0.0;
           cmd_vel->angular.z = 0.0;
          
           vel_pub_->publish(  std::move(  cmd_vel ) );
           }
          };
          
          } // namespace nav2_behaviors
          
          #endif // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_

navigation2/nav2_behaviors/plugins/back_up.cpp

       1  // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "back_up.hpp"
          
          namespace nav2_behaviors
          {
          
      20  Status BackUp::onRun(  const std::shared_ptr<const BackUpAction::Goal> command )
          {
           if (  command->target.y != 0.0 || command->target.z != 0.0 ) {
           RCLCPP_INFO(  
           logger_,  
           "Backing up in Y and Z not supported,   will only move in X." );
           return Status::FAILED;
           }
          
           // Silently ensure that both the speed and direction are negative.
           command_x_ = -std::fabs(  command->target.x );
           command_speed_ = -std::fabs(  command->speed );
           command_time_allowance_ = command->time_allowance;
          
           end_time_ = steady_clock_.now(   ) + command_time_allowance_;
          
           if (  !nav2_util::getCurrentPose(  
           initial_pose_,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_ERROR(  logger_,   "Initial robot pose is not available." );
           return Status::FAILED;
           }
          
           return Status::SUCCEEDED;
          }
          
          } // namespace nav2_behaviors
          
          #include "pluginlib/class_list_macros.hpp"
      50  PLUGINLIB_EXPORT_CLASS(  nav2_behaviors::BackUp,   nav2_core::Behavior )

navigation2/nav2_behaviors/plugins/back_up.hpp

       1  // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
          #define NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
          
          #include <memory>
          
          #include "drive_on_heading.hpp"
          #include "nav2_msgs/action/back_up.hpp"
          
          using BackUpAction = nav2_msgs::action::BackUp;
          
          
          namespace nav2_behaviors
          {
      28  class BackUp : public DriveOnHeading<nav2_msgs::action::BackUp>
          {
          public:
           Status onRun(  const std::shared_ptr<const BackUpAction::Goal> command ) override;
          };
          }
          
          #endif // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_

navigation2/nav2_behaviors/plugins/drive_on_heading.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include "drive_on_heading.hpp"
          
          #include "pluginlib/class_list_macros.hpp"
      20  PLUGINLIB_EXPORT_CLASS(  nav2_behaviors::DriveOnHeading<>,   nav2_core::Behavior )

navigation2/nav2_behaviors/plugins/drive_on_heading.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
          #define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
          
          #include <chrono>
          #include <memory>
          #include <utility>
          
          #include "nav2_behaviors/timed_behavior.hpp"
          #include "nav2_msgs/action/drive_on_heading.hpp"
          #include "nav2_msgs/action/back_up.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_behaviors
          {
          
          /**
           * @class nav2_behaviors::DriveOnHeading
           * @brief An action server Behavior for spinning in
           */
          template<typename ActionT = nav2_msgs::action::DriveOnHeading>
      36  class DriveOnHeading : public TimedBehavior<ActionT>
          {
          public:
           /**
           * @brief A constructor for nav2_behaviors::DriveOnHeading
           */
           DriveOnHeading(   )
           : TimedBehavior<ActionT>(   ),  
           feedback_(  std::make_shared<typename ActionT::Feedback>(   ) ),  
           command_x_(  0.0 ),  
           command_speed_(  0.0 ),  
           simulate_ahead_time_(  0.0 )
           {
           }
          
           ~DriveOnHeading(   ) = default;
          
           /**
           * @brief Initialization to run behavior
           * @param command Goal to execute
           * @return Status of behavior
           */
      58   Status onRun(  const std::shared_ptr<const typename ActionT::Goal> command ) override
           {
           if (  command->target.y != 0.0 || command->target.z != 0.0 ) {
           RCLCPP_INFO(  
           this->logger_,  
           "DrivingOnHeading in Y and Z not supported,   will only move in X." );
           return Status::FAILED;
           }
          
           // Ensure that both the speed and direction have the same sign
           if (  !(  (  command->target.x > 0.0 ) == (  command->speed > 0.0 ) )  ) {
           RCLCPP_ERROR(  this->logger_,   "Speed and command sign did not match" );
           return Status::FAILED;
           }
          
           command_x_ = command->target.x;
           command_speed_ = command->speed;
           command_time_allowance_ = command->time_allowance;
          
           end_time_ = this->steady_clock_.now(   ) + command_time_allowance_;
          
           if (  !nav2_util::getCurrentPose(  
           initial_pose_,   *this->tf_,   this->global_frame_,   this->robot_base_frame_,  
           this->transform_tolerance_ ) )
           {
           RCLCPP_ERROR(  this->logger_,   "Initial robot pose is not available." );
           return Status::FAILED;
           }
          
           return Status::SUCCEEDED;
           }
          
           /**
           * @brief Loop function to run behavior
           * @return Status of behavior
           */
      94   Status onCycleUpdate(   )
           {
           rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(   );
           if (  time_remaining.seconds(   ) < 0.0 && command_time_allowance_.seconds(   ) > 0.0 ) {
           this->stopRobot(   );
           RCLCPP_WARN(  
           this->logger_,  
           "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading" );
           return Status::FAILED;
           }
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *this->tf_,   this->global_frame_,   this->robot_base_frame_,  
           this->transform_tolerance_ ) )
           {
           RCLCPP_ERROR(  this->logger_,   "Current robot pose is not available." );
           return Status::FAILED;
           }
          
           double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
           double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y;
           double distance = hypot(  diff_x,   diff_y );
          
           feedback_->distance_traveled = distance;
           this->action_server_->publish_feedback(  feedback_ );
          
           if (  distance >= std::fabs(  command_x_ ) ) {
           this->stopRobot(   );
           return Status::SUCCEEDED;
           }
          
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
           cmd_vel->linear.y = 0.0;
           cmd_vel->angular.z = 0.0;
           cmd_vel->linear.x = command_speed_;
          
           geometry_msgs::msg::Pose2D pose2d;
           pose2d.x = current_pose.pose.position.x;
           pose2d.y = current_pose.pose.position.y;
           pose2d.theta = tf2::getYaw(  current_pose.pose.orientation );
          
           if (  !isCollisionFree(  distance,   cmd_vel.get(   ),   pose2d ) ) {
           this->stopRobot(   );
           RCLCPP_WARN(  this->logger_,   "Collision Ahead - Exiting DriveOnHeading" );
           return Status::FAILED;
           }
          
           this->vel_pub_->publish(  std::move(  cmd_vel ) );
          
           return Status::RUNNING;
           }
          
          protected:
           /**
           * @brief Check if pose is collision free
           * @param distance Distance to check forward
           * @param cmd_vel current commanded velocity
           * @param pose2d Current pose
           * @return is collision free or not
           */
     155   bool isCollisionFree(  
           const double & distance,  
           geometry_msgs::msg::Twist * cmd_vel,  
           geometry_msgs::msg::Pose2D & pose2d )
           {
           // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
           int cycle_count = 0;
           double sim_position_change;
           const double diff_dist = abs(  command_x_ ) - distance;
           const int max_cycle_count = static_cast<int>(  this->cycle_frequency_ * simulate_ahead_time_ );
           geometry_msgs::msg::Pose2D init_pose = pose2d;
           bool fetch_data = true;
          
           while (  cycle_count < max_cycle_count ) {
           sim_position_change = cmd_vel->linear.x * (  cycle_count / this->cycle_frequency_ );
           pose2d.x = init_pose.x + sim_position_change * cos(  init_pose.theta );
           pose2d.y = init_pose.y + sim_position_change * sin(  init_pose.theta );
           cycle_count++;
          
           if (  diff_dist - abs(  sim_position_change ) <= 0. ) {
           break;
           }
          
           if (  !this->collision_checker_->isCollisionFree(  pose2d,   fetch_data ) ) {
           return false;
           }
           fetch_data = false;
           }
           return true;
           }
          
           /**
           * @brief Configuration of behavior action
           */
     189   void onConfigure(   ) override
           {
           auto node = this->node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           "simulate_ahead_time",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  "simulate_ahead_time",   simulate_ahead_time_ );
           }
          
           typename ActionT::Feedback::SharedPtr feedback_;
          
           geometry_msgs::msg::PoseStamped initial_pose_;
           double command_x_;
           double command_speed_;
           rclcpp::Duration command_time_allowance_{0,   0};
           rclcpp::Time end_time_;
           double simulate_ahead_time_;
          };
          
          } // namespace nav2_behaviors
          
          #endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_

navigation2/nav2_behaviors/plugins/spin.cpp

       1  // Copyright (  c ) 2018 Intel Corporation,   2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <cmath>
          #include <thread>
          #include <algorithm>
          #include <memory>
          #include <utility>
          
          #include "spin.hpp"
          #include "tf2/utils.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "nav2_util/node_utils.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_behaviors
          {
          
      31  Spin::Spin(   )
          : TimedBehavior<SpinAction>(   ),  
           feedback_(  std::make_shared<SpinAction::Feedback>(   ) ),  
           min_rotational_vel_(  0.0 ),  
           max_rotational_vel_(  0.0 ),  
           rotational_acc_lim_(  0.0 ),  
           cmd_yaw_(  0.0 ),  
           prev_yaw_(  0.0 ),  
           relative_yaw_(  0.0 ),  
           simulate_ahead_time_(  0.0 )
          {
          }
          
      44  Spin::~Spin(   ) = default;
          
      46  void Spin::onConfigure(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           "simulate_ahead_time",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  "simulate_ahead_time",   simulate_ahead_time_ );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           "max_rotational_vel",   rclcpp::ParameterValue(  1.0 ) );
           node->get_parameter(  "max_rotational_vel",   max_rotational_vel_ );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           "min_rotational_vel",   rclcpp::ParameterValue(  0.4 ) );
           node->get_parameter(  "min_rotational_vel",   min_rotational_vel_ );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           "rotational_acc_lim",   rclcpp::ParameterValue(  3.2 ) );
           node->get_parameter(  "rotational_acc_lim",   rotational_acc_lim_ );
          }
          
      74  Status Spin::onRun(  const std::shared_ptr<const SpinAction::Goal> command )
          {
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_ERROR(  logger_,   "Current robot pose is not available." );
           return Status::FAILED;
           }
          
           prev_yaw_ = tf2::getYaw(  current_pose.pose.orientation );
           relative_yaw_ = 0.0;
          
           cmd_yaw_ = command->target_yaw;
           RCLCPP_INFO(  
           logger_,   "Turning %0.2f for spin behavior.",  
           cmd_yaw_ );
          
           command_time_allowance_ = command->time_allowance;
           end_time_ = steady_clock_.now(   ) + command_time_allowance_;
          
           return Status::SUCCEEDED;
          }
          
      99  Status Spin::onCycleUpdate(   )
          {
           rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(   );
           if (  time_remaining.seconds(   ) < 0.0 && command_time_allowance_.seconds(   ) > 0.0 ) {
           stopRobot(   );
           RCLCPP_WARN(  
           logger_,  
           "Exceeded time allowance before reaching the Spin goal - Exiting Spin" );
           return Status::FAILED;
           }
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   *tf_,   global_frame_,   robot_base_frame_,  
           transform_tolerance_ ) )
           {
           RCLCPP_ERROR(  logger_,   "Current robot pose is not available." );
           return Status::FAILED;
           }
          
           const double current_yaw = tf2::getYaw(  current_pose.pose.orientation );
          
           double delta_yaw = current_yaw - prev_yaw_;
           if (  abs(  delta_yaw ) > M_PI ) {
           delta_yaw = copysign(  2 * M_PI - abs(  delta_yaw ),   prev_yaw_ );
           }
          
           relative_yaw_ += delta_yaw;
           prev_yaw_ = current_yaw;
          
           feedback_->angular_distance_traveled = static_cast<float>(  relative_yaw_ );
           action_server_->publish_feedback(  feedback_ );
          
           double remaining_yaw = abs(  cmd_yaw_ ) - abs(  relative_yaw_ );
           if (  remaining_yaw < 1e-6 ) {
           stopRobot(   );
           return Status::SUCCEEDED;
           }
          
           double vel = sqrt(  2 * rotational_acc_lim_ * remaining_yaw );
           vel = std::min(  std::max(  vel,   min_rotational_vel_ ),   max_rotational_vel_ );
          
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
           cmd_vel->angular.z = copysign(  vel,   cmd_yaw_ );
          
           geometry_msgs::msg::Pose2D pose2d;
           pose2d.x = current_pose.pose.position.x;
           pose2d.y = current_pose.pose.position.y;
           pose2d.theta = tf2::getYaw(  current_pose.pose.orientation );
          
           if (  !isCollisionFree(  relative_yaw_,   cmd_vel.get(   ),   pose2d ) ) {
           stopRobot(   );
           RCLCPP_WARN(  logger_,   "Collision Ahead - Exiting Spin" );
           return Status::FAILED;
           }
          
           vel_pub_->publish(  std::move(  cmd_vel ) );
          
           return Status::RUNNING;
          }
          
     160  bool Spin::isCollisionFree(  
           const double & relative_yaw,  
     162   geometry_msgs::msg::Twist * cmd_vel,  
     163   geometry_msgs::msg::Pose2D & pose2d )
          {
           // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
           int cycle_count = 0;
           double sim_position_change;
           const int max_cycle_count = static_cast<int>(  cycle_frequency_ * simulate_ahead_time_ );
           geometry_msgs::msg::Pose2D init_pose = pose2d;
           bool fetch_data = true;
          
           while (  cycle_count < max_cycle_count ) {
           sim_position_change = cmd_vel->angular.z * (  cycle_count / cycle_frequency_ );
           pose2d.theta = init_pose.theta + sim_position_change;
           cycle_count++;
          
           if (  abs(  relative_yaw ) - abs(  sim_position_change ) <= 0. ) {
           break;
           }
          
           if (  !collision_checker_->isCollisionFree(  pose2d,   fetch_data ) ) {
           return false;
           }
           fetch_data = false;
           }
           return true;
          }
          
          } // namespace nav2_behaviors
          
          #include "pluginlib/class_list_macros.hpp"
     192  PLUGINLIB_EXPORT_CLASS(  nav2_behaviors::Spin,   nav2_core::Behavior )

navigation2/nav2_behaviors/plugins/spin.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
          #define NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
          
          #include <chrono>
          #include <string>
          #include <memory>
          
          #include "nav2_behaviors/timed_behavior.hpp"
          #include "nav2_msgs/action/spin.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          
          namespace nav2_behaviors
          {
          using SpinAction = nav2_msgs::action::Spin;
          
          /**
           * @class nav2_behaviors::Spin
           * @brief An action server behavior for spinning in
           */
      34  class Spin : public TimedBehavior<SpinAction>
          {
          public:
           /**
           * @brief A constructor for nav2_behaviors::Spin
           */
      40   Spin(   );
      41   ~Spin(   );
          
           /**
           * @brief Initialization to run behavior
           * @param command Goal to execute
           * @return Status of behavior
           */
           Status onRun(  const std::shared_ptr<const SpinAction::Goal> command ) override;
          
           /**
           * @brief Configuration of behavior action
           */
           void onConfigure(   ) override;
          
           /**
           * @brief Loop function to run behavior
           * @return Status of behavior
           */
           Status onCycleUpdate(   ) override;
          
          protected:
           /**
           * @brief Check if pose is collision free
           * @param distance Distance to check forward
           * @param cmd_vel current commanded velocity
           * @param pose2d Current pose
           * @return is collision free or not
           */
           bool isCollisionFree(  
           const double & distance,  
           geometry_msgs::msg::Twist * cmd_vel,  
           geometry_msgs::msg::Pose2D & pose2d );
          
           SpinAction::Feedback::SharedPtr feedback_;
          
           double min_rotational_vel_;
           double max_rotational_vel_;
           double rotational_acc_lim_;
           double cmd_yaw_;
           double prev_yaw_;
           double relative_yaw_;
           double simulate_ahead_time_;
           rclcpp::Duration command_time_allowance_{0,   0};
           rclcpp::Time end_time_;
          };
          
          } // namespace nav2_behaviors
          
          #endif // NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_

navigation2/nav2_behaviors/plugins/wait.cpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <memory>
          
          #include "wait.hpp"
          
          namespace nav2_behaviors
          {
          
      23  Wait::Wait(   )
          : TimedBehavior<WaitAction>(   ),  
           feedback_(  std::make_shared<WaitAction::Feedback>(   ) )
          {
          }
          
      29  Wait::~Wait(   ) = default;
          
      31  Status Wait::onRun(  const std::shared_ptr<const WaitAction::Goal> command )
          {
           wait_end_ = std::chrono::steady_clock::now(   ) +
           rclcpp::Duration(  command->time ).to_chrono<std::chrono::nanoseconds>(   );
           return Status::SUCCEEDED;
          }
          
      38  Status Wait::onCycleUpdate(   )
          {
           auto current_point = std::chrono::steady_clock::now(   );
           auto time_left =
           std::chrono::duration_cast<std::chrono::nanoseconds>(  wait_end_ - current_point ).count(   );
          
           feedback_->time_left = rclcpp::Duration(  rclcpp::Duration::from_nanoseconds(  time_left ) );
           action_server_->publish_feedback(  feedback_ );
          
           if (  time_left > 0 ) {
           return Status::RUNNING;
           } else {
           return Status::SUCCEEDED;
           }
          }
          
          } // namespace nav2_behaviors
          
          #include "pluginlib/class_list_macros.hpp"
      57  PLUGINLIB_EXPORT_CLASS(  nav2_behaviors::Wait,   nav2_core::Behavior )

navigation2/nav2_behaviors/plugins/wait.hpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
          #define NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
          
          #include <chrono>
          #include <string>
          #include <memory>
          
          #include "nav2_behaviors/timed_behavior.hpp"
          #include "nav2_msgs/action/wait.hpp"
          
          namespace nav2_behaviors
          {
          using WaitAction = nav2_msgs::action::Wait;
          
          /**
           * @class nav2_behaviors::Wait
           * @brief An action server behavior for waiting a fixed duration
           */
      33  class Wait : public TimedBehavior<WaitAction>
          {
          public:
           /**
           * @brief A constructor for nav2_behaviors::Wait
           */
      39   Wait(   );
      40   ~Wait(   );
          
           /**
           * @brief Initialization to run behavior
           * @param command Goal to execute
           * @return Status of behavior
           */
           Status onRun(  const std::shared_ptr<const WaitAction::Goal> command ) override;
          
           /**
           * @brief Loop function to run behavior
           * @return Status of behavior
           */
           Status onCycleUpdate(   ) override;
          
          protected:
           std::chrono::time_point<std::chrono::steady_clock> wait_end_;
           WaitAction::Feedback::SharedPtr feedback_;
          };
          
          } // namespace nav2_behaviors
          
          #endif // NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_

navigation2/nav2_behaviors/src/behavior_server.cpp

          // Copyright (  c ) 2018 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <utility>
          #include "nav2_util/node_utils.hpp"
          #include "nav2_behaviors/behavior_server.hpp"
          
          namespace behavior_server
          {
          
      25  BehaviorServer::BehaviorServer(  const rclcpp::NodeOptions & options )
          : LifecycleNode(  "behavior_server",   "",   options ),  
           plugin_loader_(  "nav2_core",   "nav2_core::Behavior" ),  
           default_ids_{"spin",   "backup",   "drive_on_heading",   "wait"},  
      29   default_types_{"nav2_behaviors/Spin",  
           "nav2_behaviors/BackUp",  
           "nav2_behaviors/DriveOnHeading",  
           "nav2_behaviors/Wait"}
          {
      34   declare_parameter(  
           "costmap_topic",  
           rclcpp::ParameterValue(  std::string(  "local_costmap/costmap_raw" ) ) );
      37   declare_parameter(  
           "footprint_topic",  
           rclcpp::ParameterValue(  std::string(  "local_costmap/published_footprint" ) ) );
      40   declare_parameter(  "cycle_frequency",   rclcpp::ParameterValue(  10.0 ) );
      41   declare_parameter(  "behavior_plugins",   default_ids_ );
          
      43   get_parameter(  "behavior_plugins",   behavior_ids_ );
           if (  behavior_ids_ == default_ids_ ) {
           for (  size_t i = 0; i < default_ids_.size(   ); ++i ) {
           declare_parameter(  default_ids_[i] + ".plugin",   default_types_[i] );
           }
           }
          
           declare_parameter(  
           "global_frame",  
           rclcpp::ParameterValue(  std::string(  "odom" ) ) );
           declare_parameter(  
           "robot_base_frame",  
           rclcpp::ParameterValue(  std::string(  "base_link" ) ) );
           declare_parameter(  
           "transform_tolerance",  
           rclcpp::ParameterValue(  0.1 ) );
          }
          
          
          BehaviorServer::~BehaviorServer(   )
          {
           behaviors_.clear(   );
          }
          
          nav2_util::CallbackReturn
          BehaviorServer::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           tf_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),  
           get_node_timers_interface(   ) );
           tf_->setCreateTimerInterface(  timer_interface );
           transform_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_ );
          
           std::string costmap_topic,   footprint_topic,   robot_base_frame;
           double transform_tolerance;
           this->get_parameter(  "costmap_topic",   costmap_topic );
           this->get_parameter(  "footprint_topic",   footprint_topic );
           this->get_parameter(  "transform_tolerance",   transform_tolerance );
           this->get_parameter(  "robot_base_frame",   robot_base_frame );
           costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(  
           shared_from_this(   ),   costmap_topic );
           footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(  
           shared_from_this(   ),   footprint_topic,   *tf_,   robot_base_frame,   transform_tolerance );
          
           collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(  
           *costmap_sub_,   *footprint_sub_,   this->get_name(   ) );
          
           behavior_types_.resize(  behavior_ids_.size(   ) );
           if (  !loadBehaviorPlugins(   ) ) {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          
          bool
          BehaviorServer::loadBehaviorPlugins(   )
          {
           auto node = shared_from_this(   );
          
           for (  size_t i = 0; i != behavior_ids_.size(   ); i++ ) {
           behavior_types_[i] = nav2_util::get_plugin_type_param(  node,   behavior_ids_[i] );
           try {
           RCLCPP_INFO(  
           get_logger(   ),   "Creating behavior plugin %s of type %s",  
           behavior_ids_[i].c_str(   ),   behavior_types_[i].c_str(   ) );
           behaviors_.push_back(  plugin_loader_.createUniqueInstance(  behavior_types_[i] ) );
           behaviors_.back(   )->configure(  node,   behavior_ids_[i],   tf_,   collision_checker_ );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),   "Failed to create behavior %s of type %s."
           " Exception: %s",   behavior_ids_[i].c_str(   ),   behavior_types_[i].c_str(   ),  
           ex.what(   ) );
           return false;
           }
           }
          
           return true;
          }
          
          nav2_util::CallbackReturn
          BehaviorServer::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
           std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
           for (  iter = behaviors_.begin(   ); iter != behaviors_.end(   ); ++iter ) {
           (  *iter )->activate(   );
           }
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          BehaviorServer::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
           for (  iter = behaviors_.begin(   ); iter != behaviors_.end(   ); ++iter ) {
           (  *iter )->deactivate(   );
           }
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          BehaviorServer::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
           for (  iter = behaviors_.begin(   ); iter != behaviors_.end(   ); ++iter ) {
           (  *iter )->cleanup(   );
           }
          
           behaviors_.clear(   );
           transform_listener_.reset(   );
           tf_.reset(   );
           footprint_sub_.reset(   );
           costmap_sub_.reset(   );
           collision_checker_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          BehaviorServer::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          } // end namespace behavior_server
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
          RCLCPP_COMPONENTS_REGISTER_NODE(  behavior_server::BehaviorServer )

navigation2/nav2_behaviors/src/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          
          #include "nav2_behaviors/behavior_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      21  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto recoveries_node = std::make_shared<behavior_server::BehaviorServer>(   );
          
           rclcpp::spin(  recoveries_node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_behaviors/test/test_behaviors.cpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <chrono>
          #include <iostream>
          #include <thread>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_behaviors/timed_behavior.hpp"
          #include "nav2_msgs/action/dummy_behavior.hpp"
          
          using nav2_behaviors::TimedBehavior;
          using nav2_behaviors::Status;
          using BehaviorAction = nav2_msgs::action::DummyBehavior;
          using ClientGoalHandle = rclcpp_action::ClientGoalHandle<BehaviorAction>;
          
          using namespace std::chrono_literals;
          
          // A behavior for testing the base class
          
      37  class DummyBehavior : public TimedBehavior<BehaviorAction>
          {
          public:
      40   DummyBehavior(   )
           : TimedBehavior<BehaviorAction>(   ),  
           initialized_(  false ) {}
          
           ~DummyBehavior(   ) = default;
          
           Status onRun(  const std::shared_ptr<const BehaviorAction::Goal> goal ) override
           {
           // A normal behavior would catch the command and initialize
           initialized_ = false;
           command_ = goal->command.data;
           start_time_ = std::chrono::system_clock::now(   );
          
           // onRun method can have various possible outcomes (  success,   failure,   cancelled )
           // The output is defined by the tester class on the command string.
           if (  command_ == "Testing success" || command_ == "Testing failure on run" ) {
           initialized_ = true;
           return Status::SUCCEEDED;
           }
          
           return Status::FAILED;
           }
          
           Status onCycleUpdate(   ) override
           {
           // A normal behavior would set the robot in motion in the first call
           // and check for robot states on subsequent calls to check if the movement
           // was completed.
          
           if (  command_ != "Testing success" || !initialized_ ) {
           return Status::FAILED;
           }
          
           // For testing,   pretend the robot takes some fixed
           // amount of time to complete the motion.
           auto current_time = std::chrono::system_clock::now(   );
           auto motion_duration = 1s;
          
           if (  current_time - start_time_ >= motion_duration ) {
           // Movement was completed
           return Status::SUCCEEDED;
           }
          
           return Status::RUNNING;
           }
          
          private:
           bool initialized_;
           std::string command_;
           std::chrono::system_clock::time_point start_time_;
          };
          
          // Define a test class to hold the context for the tests
          
          class BehaviorTest : public ::testing::Test
          {
          protected:
           BehaviorTest(   ) {SetUp(   );}
           ~BehaviorTest(   ) = default;
          
           void SetUp(   ) override
           {
           node_lifecycle_ =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  
           "LifecycleBehaviorTestNode",   rclcpp::NodeOptions(   ) );
           node_lifecycle_->declare_parameter(  
           "costmap_topic",  
           rclcpp::ParameterValue(  std::string(  "local_costmap/costmap_raw" ) ) );
           node_lifecycle_->declare_parameter(  
           "footprint_topic",  
           rclcpp::ParameterValue(  std::string(  "local_costmap/published_footprint" ) ) );
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_lifecycle_->get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           node_lifecycle_->get_node_base_interface(   ),  
           node_lifecycle_->get_node_timers_interface(   ) );
           tf_buffer_->setCreateTimerInterface(  timer_interface );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
          
           std::string costmap_topic,   footprint_topic;
           node_lifecycle_->get_parameter(  "costmap_topic",   costmap_topic );
           node_lifecycle_->get_parameter(  "footprint_topic",   footprint_topic );
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_ =
           std::make_shared<nav2_costmap_2d::CostmapSubscriber>(  
           node_lifecycle_,   costmap_topic );
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
           std::make_shared<nav2_costmap_2d::FootprintSubscriber>(  
           node_lifecycle_,   footprint_topic,   *tf_buffer_ );
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_ =
           std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(  
           *costmap_sub_,   *footprint_sub_,  
           node_lifecycle_->get_name(   ) );
          
           behavior_ = std::make_shared<DummyBehavior>(   );
           behavior_->configure(  node_lifecycle_,   "Behavior",   tf_buffer_,   collision_checker_ );
           behavior_->activate(   );
          
           client_ = rclcpp_action::create_client<BehaviorAction>(  
           node_lifecycle_->get_node_base_interface(   ),  
           node_lifecycle_->get_node_graph_interface(   ),  
           node_lifecycle_->get_node_logging_interface(   ),  
           node_lifecycle_->get_node_waitables_interface(   ),   "Behavior" );
           std::cout << "Setup complete." << std::endl;
           }
          
           void TearDown(   ) override {}
          
           bool sendCommand(  const std::string & command )
           {
           if (  !client_->wait_for_action_server(  4s ) ) {
           std::cout << "Server not up" << std::endl;
           return false;
           }
          
           auto goal = BehaviorAction::Goal(   );
           goal.command.data = command;
           auto future_goal = client_->async_send_goal(  goal );
          
           if (  rclcpp::spin_until_future_complete(  node_lifecycle_,   future_goal ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           std::cout << "failed sending goal" << std::endl;
           // failed sending the goal
           return false;
           }
          
           goal_handle_ = future_goal.get(   );
          
           if (  !goal_handle_ ) {
           std::cout << "goal was rejected" << std::endl;
           // goal was rejected by the action server
           return false;
           }
          
           return true;
           }
          
           Status getOutcome(   )
           {
           if (  getResult(   ).code == rclcpp_action::ResultCode::SUCCEEDED ) {
           return Status::SUCCEEDED;
           }
          
           return Status::FAILED;
           }
          
           ClientGoalHandle::WrappedResult getResult(   )
           {
           std::cout << "Getting async result..." << std::endl;
           auto future_result = client_->async_get_result(  goal_handle_ );
           std::cout << "Waiting on future..." << std::endl;
           rclcpp::spin_until_future_complete(  node_lifecycle_,   future_result );
           std::cout << "future received!" << std::endl;
           return future_result.get(   );
           }
          
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
           std::shared_ptr<DummyBehavior> behavior_;
           std::shared_ptr<rclcpp_action::Client<BehaviorAction>> client_;
           std::shared_ptr<rclcpp_action::ClientGoalHandle<BehaviorAction>> goal_handle_;
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          };
          
          // Define the tests
          
          TEST_F(  BehaviorTest,   testingSuccess )
          {
           ASSERT_TRUE(  sendCommand(  "Testing success" ) );
           EXPECT_EQ(  getOutcome(   ),   Status::SUCCEEDED );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingFailureOnRun )
          {
           ASSERT_TRUE(  sendCommand(  "Testing failure on run" ) );
           EXPECT_EQ(  getOutcome(   ),   Status::FAILED );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingFailureOnInit )
          {
           ASSERT_TRUE(  sendCommand(  "Testing failure on init" ) );
           EXPECT_EQ(  getOutcome(   ),   Status::FAILED );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingSequentialFailures )
          {
           ASSERT_TRUE(  sendCommand(  "Testing failure on run" ) );
           EXPECT_EQ(  getOutcome(   ),   Status::FAILED );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingTotalElapsedTimeIsGratherThanZeroIfStarted )
          {
           ASSERT_TRUE(  sendCommand(  "Testing success" ) );
           EXPECT_GT(  getResult(   ).result->total_elapsed_time.sec,   0.0 );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingTotalElapsedTimeIsZeroIfFailureOnInit )
          {
           ASSERT_TRUE(  sendCommand(  "Testing failure on init" ) );
           EXPECT_EQ(  getResult(   ).result->total_elapsed_time.sec,   0.0 );
           SUCCEED(   );
          }
          
          TEST_F(  BehaviorTest,   testingTotalElapsedTimeIsZeroIfFailureOnRun )
          {
           ASSERT_TRUE(  sendCommand(  "Testing failure on run" ) );
           EXPECT_EQ(  getResult(   ).result->total_elapsed_time.sec,   0.0 );
           SUCCEED(   );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
          #define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/odometry_utils.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
          #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
          
          namespace nav2_bt_navigator
          {
          
          /**
           * @class nav2_bt_navigator::BtNavigator
           * @brief An action server that uses behavior tree for navigating a robot to its
           * goal position.
           */
      39  class BtNavigator : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief A constructor for nav2_bt_navigator::BtNavigator class
           * @param options Additional options to control creation of the node.
           */
      46   explicit BtNavigator(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief A destructor for nav2_bt_navigator::BtNavigator class
           */
      50   ~BtNavigator(   );
          
          protected:
           /**
           * @brief Configures member variables
           *
           * Initializes action server for "NavigationToPose"; subscription to
           * "goal_sub"; and builds behavior tree from xml file.
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Activates action server
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Deactivates action server
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Resets member variables
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in shutdown state
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           // To handle all the BT related execution
           std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>> pose_navigator_;
           std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>>
           poses_navigator_;
           nav2_bt_navigator::NavigatorMuxer plugin_muxer_;
          
           // Odometry smoother object
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
          
           // Metrics for feedback
           std::string robot_frame_;
           std::string global_frame_;
           double transform_tolerance_;
           std::string odom_topic_;
          
           // Spinning transform that can be used by the BT nodes
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          };
          
          } // namespace nav2_bt_navigator
          
          #endif // NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_

navigation2/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
          #define NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <mutex>
          
          #include "nav2_util/odometry_utils.hpp"
          #include "tf2_ros/buffer.h"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "nav2_behavior_tree/bt_action_server.hpp"
          
          namespace nav2_bt_navigator
          {
          
          /**
           * @struct FeedbackUtils
           * @brief Navigator feedback utilities required to get transforms and reference frames.
           */
          struct FeedbackUtils
          {
           std::string robot_frame;
           std::string global_frame;
           double transform_tolerance;
           std::shared_ptr<tf2_ros::Buffer> tf;
          };
          
          /**
           * @class NavigatorMuxer
           * @brief A class to control the state of the BT navigator by allowing only a single
           * plugin to be processed at a time.
           */
      50  class NavigatorMuxer
          {
          public:
           /**
           * @brief A Navigator Muxer constructor
           */
      56   NavigatorMuxer(   )
           : current_navigator_(  std::string(  "" ) ) {}
          
           /**
           * @brief Get the navigator muxer state
           * @return bool If a navigator is in progress
           */
      63   bool isNavigating(   )
           {
           std::scoped_lock l(  mutex_ );
           return !current_navigator_.empty(   );
           }
          
           /**
           * @brief Start navigating with a given navigator
           * @param string Name of the navigator to start
           */
      73   void startNavigating(  const std::string & navigator_name )
           {
           std::scoped_lock l(  mutex_ );
           if (  !current_navigator_.empty(   ) ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "NavigatorMutex" ),  
           "Major error! Navigation requested while another navigation"
           " task is in progress! This likely occurred from an incorrect"
           "implementation of a navigator plugin." );
           }
           current_navigator_ = navigator_name;
           }
          
           /**
           * @brief Stop navigating with a given navigator
           * @param string Name of the navigator ending task
           */
      90   void stopNavigating(  const std::string & navigator_name )
           {
           std::scoped_lock l(  mutex_ );
           if (  current_navigator_ != navigator_name ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "NavigatorMutex" ),  
           "Major error! Navigation stopped while another navigation"
           " task is in progress! This likely occurred from an incorrect"
           "implementation of a navigator plugin." );
           } else {
           current_navigator_ = std::string(  "" );
           }
           }
          
          protected:
     105   std::string current_navigator_;
     106   std::mutex mutex_;
          };
          
          /**
           * @class Navigator
           * @brief Navigator interface that acts as a base class for all BT-based Navigator action's plugins
           */
          template<class ActionT>
     114  class Navigator
          {
          public:
           using Ptr = std::shared_ptr<nav2_bt_navigator::Navigator<ActionT>>;
          
           /**
           * @brief A Navigator constructor
           */
           Navigator(   )
           {
           plugin_muxer_ = nullptr;
           }
          
           /**
           * @brief Virtual destructor
           */
           virtual ~Navigator(   ) = default;
          
           /**
           * @brief Configuration to setup the navigator's backend BT and actions
           * @param parent_node The ROS parent node to utilize
           * @param plugin_lib_names a vector of plugin shared libraries to load
           * @param feedback_utils Some utilities useful for navigators to have
           * @param plugin_muxer The muxing object to ensure only one navigator
           * can be active at a time
           * @param odom_smoother Object to get current smoothed robot's speed
           * @return bool If successful
           */
     142   bool on_configure(  
           rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,  
           const std::vector<std::string> & plugin_lib_names,  
           const FeedbackUtils & feedback_utils,  
           nav2_bt_navigator::NavigatorMuxer * plugin_muxer,  
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
           {
           auto node = parent_node.lock(   );
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
           feedback_utils_ = feedback_utils;
           plugin_muxer_ = plugin_muxer;
          
           // get the default behavior tree for this navigator
           std::string default_bt_xml_filename = getDefaultBTFilepath(  parent_node );
          
           // Create the Behavior Tree Action Server for this navigator
           bt_action_server_ = std::make_unique<nav2_behavior_tree::BtActionServer<ActionT>>(  
           node,  
           getName(   ),  
           plugin_lib_names,  
           default_bt_xml_filename,  
           std::bind(  &Navigator::onGoalReceived,   this,   std::placeholders::_1 ),  
           std::bind(  &Navigator::onLoop,   this ),  
           std::bind(  &Navigator::onPreempt,   this,   std::placeholders::_1 ),  
           std::bind(  &Navigator::onCompletion,   this,   std::placeholders::_1,   std::placeholders::_2 ) );
          
           bool ok = true;
           if (  !bt_action_server_->on_configure(   ) ) {
           ok = false;
           }
          
           BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard(   );
           blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer",   feedback_utils.tf ); // NOLINT
           blackboard->set<bool>(  "initial_pose_received",   false ); // NOLINT
           blackboard->set<int>(  "number_recoveries",   0 ); // NOLINT
          
           return configure(  parent_node,   odom_smoother ) && ok;
           }
          
           /**
           * @brief Activation of the navigator's backend BT and actions
           * @return bool If successful
           */
     186   bool on_activate(   )
           {
           bool ok = true;
          
           if (  !bt_action_server_->on_activate(   ) ) {
           ok = false;
           }
          
           return activate(   ) && ok;
           }
          
           /**
           * @brief Deactivation of the navigator's backend BT and actions
           * @return bool If successful
           */
     201   bool on_deactivate(   )
           {
           bool ok = true;
           if (  !bt_action_server_->on_deactivate(   ) ) {
           ok = false;
           }
          
           return deactivate(   ) && ok;
           }
          
           /**
           * @brief Cleanup a navigator
           * @return bool If successful
           */
     215   bool on_cleanup(   )
           {
           bool ok = true;
           if (  !bt_action_server_->on_cleanup(   ) ) {
           ok = false;
           }
          
           bt_action_server_.reset(   );
          
           return cleanup(   ) && ok;
           }
          
           /**
           * @brief Get the action name of this navigator to expose
           * @return string Name of action to expose
           */
     231   virtual std::string getName(   ) = 0;
          
           virtual std::string getDefaultBTFilepath(  rclcpp_lifecycle::LifecycleNode::WeakPtr node ) = 0;
          
           /**
           * @brief Get the action server
           * @return Action server pointer
           */
           std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> & getActionServer(   )
           {
           return bt_action_server_;
           }
          
          protected:
           /**
           * @brief An intermediate goal reception function to mux navigators.
           */
           bool onGoalReceived(  typename ActionT::Goal::ConstSharedPtr goal )
           {
           if (  plugin_muxer_->isNavigating(   ) ) {
           RCLCPP_ERROR(  
           logger_,  
           "Requested navigation from %s while another navigator is processing,  "
           " rejecting request.",   getName(   ).c_str(   ) );
           return false;
           }
          
           bool goal_accepted = goalReceived(  goal );
          
           if (  goal_accepted ) {
           plugin_muxer_->startNavigating(  getName(   ) );
           }
          
           return goal_accepted;
           }
          
           /**
           * @brief An intermediate completion function to mux navigators
           */
           void onCompletion(  
           typename ActionT::Result::SharedPtr result,  
           const nav2_behavior_tree::BtStatus final_bt_status )
           {
           plugin_muxer_->stopNavigating(  getName(   ) );
           goalCompleted(  result,   final_bt_status );
           }
          
           /**
           * @brief A callback to be called when a new goal is received by the BT action server
           * Can be used to check if goal is valid and put values on
           * the blackboard which depend on the received goal
           */
           virtual bool goalReceived(  typename ActionT::Goal::ConstSharedPtr goal ) = 0;
          
           /**
           * @brief A callback that defines execution that happens on one iteration through the BT
           * Can be used to publish action feedback
           */
           virtual void onLoop(   ) = 0;
          
           /**
           * @brief A callback that is called when a preempt is requested
           */
           virtual void onPreempt(  typename ActionT::Goal::ConstSharedPtr goal ) = 0;
          
           /**
           * @brief A callback that is called when a the action is completed; Can fill in
           * action result message or indicate that this action is done.
           */
           virtual void goalCompleted(  
           typename ActionT::Result::SharedPtr result,  
           const nav2_behavior_tree::BtStatus final_bt_status ) = 0;
          
           /**
           * @param Method to configure resources.
           */
           virtual bool configure(  
           rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/,  
           std::shared_ptr<nav2_util::OdomSmoother>/*odom_smoother*/ )
           {
           return true;
           }
          
           /**
           * @brief Method to cleanup resources.
           */
           virtual bool cleanup(   ) {return true;}
          
           /**
           * @brief Method to activate any threads involved in execution.
           */
           virtual bool activate(   ) {return true;}
          
           /**
           * @brief Method to deactivate and any threads involved in execution.
           */
           virtual bool deactivate(   ) {return true;}
          
           std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> bt_action_server_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "Navigator" )};
           rclcpp::Clock::SharedPtr clock_;
           FeedbackUtils feedback_utils_;
           NavigatorMuxer * plugin_muxer_;
          };
          
          } // namespace nav2_bt_navigator
          
          #endif // NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_

navigation2/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

          // Copyright (  c ) 2021 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
          #define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_bt_navigator/navigator.hpp"
          #include "nav2_msgs/action/navigate_through_poses.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/odometry_utils.hpp"
          
          namespace nav2_bt_navigator
          {
          
          /**
           * @class NavigateToPoseNavigator
           * @brief A navigator for navigating to a a bunch of intermediary poses
           */
      38  class NavigateThroughPosesNavigator
      39   : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>
          {
          public:
           using ActionT = nav2_msgs::action::NavigateThroughPoses;
           typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
          
           /**
           * @brief A constructor for NavigateThroughPosesNavigator
           */
           NavigateThroughPosesNavigator(   )
           : Navigator(   ) {}
          
           /**
           * @brief A configure state transition to configure navigator's state
           * @param node Weakptr to the lifecycle node
           * @param odom_smoother Object to get current smoothed robot's speed
           */
           bool configure(  
           rclcpp_lifecycle::LifecycleNode::WeakPtr node,  
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother ) override;
          
           /**
           * @brief Get action name for this navigator
           * @return string Name of action server
           */
      64   std::string getName(   ) override {return std::string(  "navigate_through_poses" );}
          
           /**
           * @brief Get navigator's default BT
           * @param node WeakPtr to the lifecycle node
           * @return string Filepath to default XML
           */
      71   std::string getDefaultBTFilepath(  rclcpp_lifecycle::LifecycleNode::WeakPtr node ) override;
          
          protected:
           /**
           * @brief A callback to be called when a new goal is received by the BT action server
           * Can be used to check if goal is valid and put values on
           * the blackboard which depend on the received goal
           * @param goal Action template's goal message
           * @return bool if goal was received successfully to be processed
           */
      81   bool goalReceived(  ActionT::Goal::ConstSharedPtr goal ) override;
          
           /**
           * @brief A callback that defines execution that happens on one iteration through the BT
           * Can be used to publish action feedback
           */
      87   void onLoop(   ) override;
          
           /**
           * @brief A callback that is called when a preempt is requested
           */
      92   void onPreempt(  ActionT::Goal::ConstSharedPtr goal ) override;
          
           /**
           * @brief A callback that is called when a the action is completed,   can fill in
           * action result message or indicate that this action is done.
           * @param result Action template result message to populate
           * @param final_bt_status Resulting status of the behavior tree execution that may be
           * referenced while populating the result.
           */
     101   void goalCompleted(  
           typename ActionT::Result::SharedPtr result,  
           const nav2_behavior_tree::BtStatus final_bt_status ) override;
          
           /**
           * @brief Goal pose initialization on the blackboard
           */
     108   void initializeGoalPoses(  ActionT::Goal::ConstSharedPtr goal );
          
           rclcpp::Time start_time_;
           std::string goals_blackboard_id_;
           std::string path_blackboard_id_;
          
           // Odometry smoother object
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
          };
          
          } // namespace nav2_bt_navigator
          
          #endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_

navigation2/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

          // Copyright (  c ) 2021 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
          #define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_bt_navigator/navigator.hpp"
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/odometry_utils.hpp"
          
          namespace nav2_bt_navigator
          {
          
          /**
           * @class NavigateToPoseNavigator
           * @brief A navigator for navigating to a specified pose
           */
      38  class NavigateToPoseNavigator
      39   : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>
          {
          public:
           using ActionT = nav2_msgs::action::NavigateToPose;
          
           /**
           * @brief A constructor for NavigateToPoseNavigator
           */
           NavigateToPoseNavigator(   )
           : Navigator(   ) {}
          
           /**
           * @brief A configure state transition to configure navigator's state
           * @param node Weakptr to the lifecycle node
           * @param odom_smoother Object to get current smoothed robot's speed
           */
           bool configure(  
           rclcpp_lifecycle::LifecycleNode::WeakPtr node,  
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother ) override;
          
           /**
           * @brief A cleanup state transition to remove memory allocated
           */
      62   bool cleanup(   ) override;
          
           /**
           * @brief A subscription and callback to handle the topic-based goal published
           * from rviz
           * @param pose Pose received via atopic
           */
      69   void onGoalPoseReceived(  const geometry_msgs::msg::PoseStamped::SharedPtr pose );
          
           /**
           * @brief Get action name for this navigator
           * @return string Name of action server
           */
      75   std::string getName(   ) {return std::string(  "navigate_to_pose" );}
          
           /**
           * @brief Get navigator's default BT
           * @param node WeakPtr to the lifecycle node
           * @return string Filepath to default XML
           */
      82   std::string getDefaultBTFilepath(  rclcpp_lifecycle::LifecycleNode::WeakPtr node ) override;
          
          protected:
           /**
           * @brief A callback to be called when a new goal is received by the BT action server
           * Can be used to check if goal is valid and put values on
           * the blackboard which depend on the received goal
           * @param goal Action template's goal message
           * @return bool if goal was received successfully to be processed
           */
      92   bool goalReceived(  ActionT::Goal::ConstSharedPtr goal ) override;
          
           /**
           * @brief A callback that defines execution that happens on one iteration through the BT
           * Can be used to publish action feedback
           */
      98   void onLoop(   ) override;
          
           /**
           * @brief A callback that is called when a preempt is requested
           */
     103   void onPreempt(  ActionT::Goal::ConstSharedPtr goal ) override;
          
           /**
           * @brief A callback that is called when a the action is completed,   can fill in
           * action result message or indicate that this action is done.
           * @param result Action template result message to populate
           * @param final_bt_status Resulting status of the behavior tree execution that may be
           * referenced while populating the result.
           */
     112   void goalCompleted(  
           typename ActionT::Result::SharedPtr result,  
           const nav2_behavior_tree::BtStatus final_bt_status ) override;
          
           /**
           * @brief Goal pose initialization on the blackboard
           * @param goal Action template's goal message to process
           */
     120   void initializeGoalPose(  ActionT::Goal::ConstSharedPtr goal );
          
           rclcpp::Time start_time_;
          
           rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
           rclcpp_action::Client<ActionT>::SharedPtr self_client_;
          
           std::string goal_blackboard_id_;
           std::string path_blackboard_id_;
          
           // Odometry smoother object
           std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
          };
          
          } // namespace nav2_bt_navigator
          
          #endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_

navigation2/nav2_bt_navigator/src/bt_navigator.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_bt_navigator/bt_navigator.hpp"
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <set>
          #include <limits>
          #include <vector>
          
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_behavior_tree/bt_conversions.hpp"
          
          namespace nav2_bt_navigator
          {
          
      31  BtNavigator::BtNavigator(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "bt_navigator",   "",   options )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           const std::vector<std::string> plugin_libs = {
           "nav2_compute_path_to_pose_action_bt_node",  
           "nav2_compute_path_through_poses_action_bt_node",  
           "nav2_smooth_path_action_bt_node",  
           "nav2_follow_path_action_bt_node",  
           "nav2_spin_action_bt_node",  
           "nav2_wait_action_bt_node",  
           "nav2_back_up_action_bt_node",  
           "nav2_drive_on_heading_bt_node",  
           "nav2_clear_costmap_service_bt_node",  
           "nav2_is_stuck_condition_bt_node",  
           "nav2_goal_reached_condition_bt_node",  
           "nav2_initial_pose_received_condition_bt_node",  
           "nav2_goal_updated_condition_bt_node",  
           "nav2_globally_updated_goal_condition_bt_node",  
           "nav2_is_path_valid_condition_bt_node",  
           "nav2_reinitialize_global_localization_service_bt_node",  
           "nav2_rate_controller_bt_node",  
           "nav2_distance_controller_bt_node",  
           "nav2_speed_controller_bt_node",  
           "nav2_truncate_path_action_bt_node",  
           "nav2_truncate_path_local_action_bt_node",  
           "nav2_goal_updater_node_bt_node",  
           "nav2_recovery_node_bt_node",  
           "nav2_pipeline_sequence_bt_node",  
           "nav2_round_robin_node_bt_node",  
           "nav2_transform_available_condition_bt_node",  
           "nav2_time_expired_condition_bt_node",  
           "nav2_path_expiring_timer_condition",  
           "nav2_distance_traveled_condition_bt_node",  
           "nav2_single_trigger_bt_node",  
           "nav2_goal_updated_controller_bt_node",  
           "nav2_is_battery_low_condition_bt_node",  
           "nav2_navigate_through_poses_action_bt_node",  
           "nav2_navigate_to_pose_action_bt_node",  
           "nav2_remove_passed_goals_action_bt_node",  
           "nav2_planner_selector_bt_node",  
           "nav2_controller_selector_bt_node",  
           "nav2_goal_checker_selector_bt_node",  
           "nav2_controller_cancel_bt_node",  
           "nav2_path_longer_on_approach_bt_node"
           "nav2_wait_cancel_bt_node",  
           "nav2_spin_cancel_bt_node",  
           "nav2_back_up_cancel_bt_node"
           "nav2_drive_on_heading_cancel_bt_node"
           };
          
           declare_parameter(  "plugin_lib_names",   plugin_libs );
           declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter(  "global_frame",   std::string(  "map" ) );
           declare_parameter(  "robot_base_frame",   std::string(  "base_link" ) );
           declare_parameter(  "odom_topic",   std::string(  "odom" ) );
          }
          
      90  BtNavigator::~BtNavigator(   )
          {
          }
          
          nav2_util::CallbackReturn
      95  BtNavigator::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           tf_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),   get_node_timers_interface(   ) );
           tf_->setCreateTimerInterface(  timer_interface );
           tf_->setUsingDedicatedThread(  true );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_,   this,   false );
          
           global_frame_ = get_parameter(  "global_frame" ).as_string(   );
           robot_frame_ = get_parameter(  "robot_base_frame" ).as_string(   );
           transform_tolerance_ = get_parameter(  "transform_tolerance" ).as_double(   );
           odom_topic_ = get_parameter(  "odom_topic" ).as_string(   );
          
           // Libraries to pull plugins (  BT Nodes ) from
           auto plugin_lib_names = get_parameter(  "plugin_lib_names" ).as_string_array(   );
          
           pose_navigator_ = std::make_unique<nav2_bt_navigator::NavigateToPoseNavigator>(   );
           poses_navigator_ = std::make_unique<nav2_bt_navigator::NavigateThroughPosesNavigator>(   );
          
           nav2_bt_navigator::FeedbackUtils feedback_utils;
           feedback_utils.tf = tf_;
           feedback_utils.global_frame = global_frame_;
           feedback_utils.robot_frame = robot_frame_;
           feedback_utils.transform_tolerance = transform_tolerance_;
          
           // Odometry smoother object for getting current speed
           odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(  shared_from_this(   ),   0.3,   odom_topic_ );
          
           if (  !pose_navigator_->on_configure(  
           shared_from_this(   ),   plugin_lib_names,   feedback_utils,   &plugin_muxer_,   odom_smoother_ ) )
           {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           if (  !poses_navigator_->on_configure(  
           shared_from_this(   ),   plugin_lib_names,   feedback_utils,   &plugin_muxer_,   odom_smoother_ ) )
           {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     142  BtNavigator::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           if (  !poses_navigator_->on_activate(   ) || !pose_navigator_->on_activate(   ) ) {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     157  BtNavigator::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           if (  !poses_navigator_->on_deactivate(   ) || !pose_navigator_->on_deactivate(   ) ) {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     172  BtNavigator::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           // Reset the listener before the buffer
           tf_listener_.reset(   );
           tf_.reset(   );
          
           if (  !poses_navigator_->on_cleanup(   ) || !pose_navigator_->on_cleanup(   ) ) {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           poses_navigator_.reset(   );
           pose_navigator_.reset(   );
          
           RCLCPP_INFO(  get_logger(   ),   "Completed Cleaning up" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     192  BtNavigator::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          } // namespace nav2_bt_navigator
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
     205  RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_bt_navigator::BtNavigator )

navigation2/nav2_bt_navigator/src/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_bt_navigator/bt_navigator.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_bt_navigator::BtNavigator>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

       1  // Copyright (  c ) 2021 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <string>
          #include <set>
          #include <memory>
          #include <limits>
          #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
          
          namespace nav2_bt_navigator
          {
          
          bool
      26  NavigateThroughPosesNavigator::configure(  
      27   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,  
      28   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
          {
           start_time_ = rclcpp::Time(  0 );
           auto node = parent_node.lock(   );
          
           if (  !node->has_parameter(  "goals_blackboard_id" ) ) {
           node->declare_parameter(  "goals_blackboard_id",   std::string(  "goals" ) );
           }
          
           goals_blackboard_id_ = node->get_parameter(  "goals_blackboard_id" ).as_string(   );
          
           if (  !node->has_parameter(  "path_blackboard_id" ) ) {
           node->declare_parameter(  "path_blackboard_id",   std::string(  "path" ) );
           }
          
           path_blackboard_id_ = node->get_parameter(  "path_blackboard_id" ).as_string(   );
          
           // Odometry smoother object for getting current speed
           odom_smoother_ = odom_smoother;
          
           return true;
          }
          
          std::string
      52  NavigateThroughPosesNavigator::getDefaultBTFilepath(  
      53   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node )
          {
           std::string default_bt_xml_filename;
           auto node = parent_node.lock(   );
          
           if (  !node->has_parameter(  "default_nav_through_poses_bt_xml" ) ) {
           std::string pkg_share_dir =
           ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           node->declare_parameter<std::string>(  
           "default_nav_through_poses_bt_xml",  
           pkg_share_dir +
           "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml" );
           }
          
           node->get_parameter(  "default_nav_through_poses_bt_xml",   default_bt_xml_filename );
          
           return default_bt_xml_filename;
          }
          
          bool
      73  NavigateThroughPosesNavigator::goalReceived(  ActionT::Goal::ConstSharedPtr goal )
          {
           auto bt_xml_filename = goal->behavior_tree;
          
           if (  !bt_action_server_->loadBehaviorTree(  bt_xml_filename ) ) {
           RCLCPP_ERROR(  
           logger_,   "Error loading XML file: %s. Navigation canceled.",  
           bt_xml_filename.c_str(   ) );
           return false;
           }
          
           initializeGoalPoses(  goal );
          
           return true;
          }
          
          void
      90  NavigateThroughPosesNavigator::goalCompleted(  
           typename ActionT::Result::SharedPtr /*result*/,  
           const nav2_behavior_tree::BtStatus /*final_bt_status*/ )
          {
          }
          
          void
      97  NavigateThroughPosesNavigator::onLoop(   )
          {
           using namespace nav2_util::geometry_utils; // NOLINT
          
           // action server feedback (  pose,   duration of task,  
           // number of recoveries,   and distance remaining to goal,   etc )
           auto feedback_msg = std::make_shared<ActionT::Feedback>(   );
          
           auto blackboard = bt_action_server_->getBlackboard(   );
          
           Goals goal_poses;
           blackboard->get<Goals>(  goals_blackboard_id_,   goal_poses );
          
           if (  goal_poses.size(   ) == 0 ) {
           bt_action_server_->publishFeedback(  feedback_msg );
           return;
           }
          
           geometry_msgs::msg::PoseStamped current_pose;
           nav2_util::getCurrentPose(  
           current_pose,   *feedback_utils_.tf,  
           feedback_utils_.global_frame,   feedback_utils_.robot_frame,  
           feedback_utils_.transform_tolerance );
          
           try {
           // Get current path points
           nav_msgs::msg::Path current_path;
           blackboard->get<nav_msgs::msg::Path>(  path_blackboard_id_,   current_path );
          
           // Find the closest pose to current pose on global path
           auto find_closest_pose_idx =
           [&current_pose,   &current_path](   ) {
           size_t closest_pose_idx = 0;
           double curr_min_dist = std::numeric_limits<double>::max(   );
           for (  size_t curr_idx = 0; curr_idx < current_path.poses.size(   ); ++curr_idx ) {
           double curr_dist = nav2_util::geometry_utils::euclidean_distance(  
           current_pose,   current_path.poses[curr_idx] );
           if (  curr_dist < curr_min_dist ) {
           curr_min_dist = curr_dist;
           closest_pose_idx = curr_idx;
           }
           }
           return closest_pose_idx;
           };
          
           // Calculate distance on the path
           double distance_remaining =
           nav2_util::geometry_utils::calculate_path_length(  current_path,   find_closest_pose_idx(   ) );
          
           // Default value for time remaining
           rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(  0.0 );
          
           // Get current speed
           geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(   );
           double current_linear_speed = std::hypot(  current_odom.linear.x,   current_odom.linear.y );
          
           // Calculate estimated time taken to goal if speed is higher than 1cm/s
           // and at least 10cm to go
           if (  (  std::abs(  current_linear_speed ) > 0.01 ) && (  distance_remaining > 0.1 ) ) {
           estimated_time_remaining =
           rclcpp::Duration::from_seconds(  distance_remaining / std::abs(  current_linear_speed ) );
           }
          
           feedback_msg->distance_remaining = distance_remaining;
           feedback_msg->estimated_time_remaining = estimated_time_remaining;
           } catch (  ... ) {
           // Ignore
           }
          
           int recovery_count = 0;
           blackboard->get<int>(  "number_recoveries",   recovery_count );
           feedback_msg->number_of_recoveries = recovery_count;
           feedback_msg->current_pose = current_pose;
           feedback_msg->navigation_time = clock_->now(   ) - start_time_;
           feedback_msg->number_of_poses_remaining = goal_poses.size(   );
          
           bt_action_server_->publishFeedback(  feedback_msg );
          }
          
          void
     177  NavigateThroughPosesNavigator::onPreempt(  ActionT::Goal::ConstSharedPtr goal )
          {
           RCLCPP_INFO(  logger_,   "Received goal preemption request" );
          
           if (  goal->behavior_tree == bt_action_server_->getCurrentBTFilename(   ) ||
           (  goal->behavior_tree.empty(   ) &&
           bt_action_server_->getCurrentBTFilename(   ) == bt_action_server_->getDefaultBTFilename(   ) ) )
           {
           // if pending goal requests the same BT as the current goal,   accept the pending goal
           // if pending goal has an empty behavior_tree field,   it requests the default BT file
           // accept the pending goal if the current goal is running the default BT file
           initializeGoalPoses(  bt_action_server_->acceptPendingGoal(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "Preemption request was rejected since the requested BT XML file is not the same "
           "as the one that the current goal is executing. Preemption with a new BT is invalid "
           "since it would require cancellation of the previous goal instead of true preemption."
           "\nCancel the current goal and send a new action request if you want to use a "
           "different BT XML file. For now,   continuing to track the last goal until completion." );
           bt_action_server_->terminatePendingGoal(   );
           }
          }
          
          void
     202  NavigateThroughPosesNavigator::initializeGoalPoses(  ActionT::Goal::ConstSharedPtr goal )
          {
           if (  goal->poses.size(   ) > 0 ) {
           RCLCPP_INFO(  
           logger_,   "Begin navigating from current location through %zu poses to (  %.2f,   %.2f )",  
           goal->poses.size(   ),   goal->poses.back(   ).pose.position.x,   goal->poses.back(   ).pose.position.y );
           }
          
           // Reset state for new action feedback
           start_time_ = clock_->now(   );
           auto blackboard = bt_action_server_->getBlackboard(   );
           blackboard->set<int>(  "number_recoveries",   0 ); // NOLINT
          
           // Update the goal pose on the blackboard
           blackboard->set<Goals>(  goals_blackboard_id_,   goal->poses );
          }
          
          } // namespace nav2_bt_navigator

navigation2/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

       1  // Copyright (  c ) 2021 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <string>
          #include <set>
          #include <memory>
          #include <limits>
          #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
          
          namespace nav2_bt_navigator
          {
          
          bool
      26  NavigateToPoseNavigator::configure(  
      27   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,  
      28   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
          {
           start_time_ = rclcpp::Time(  0 );
           auto node = parent_node.lock(   );
          
           if (  !node->has_parameter(  "goal_blackboard_id" ) ) {
           node->declare_parameter(  "goal_blackboard_id",   std::string(  "goal" ) );
           }
          
           goal_blackboard_id_ = node->get_parameter(  "goal_blackboard_id" ).as_string(   );
          
           if (  !node->has_parameter(  "path_blackboard_id" ) ) {
           node->declare_parameter(  "path_blackboard_id",   std::string(  "path" ) );
           }
          
           path_blackboard_id_ = node->get_parameter(  "path_blackboard_id" ).as_string(   );
          
           // Odometry smoother object for getting current speed
           odom_smoother_ = odom_smoother;
          
           self_client_ = rclcpp_action::create_client<ActionT>(  node,   getName(   ) );
          
           goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(  
           "goal_pose",  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &NavigateToPoseNavigator::onGoalPoseReceived,   this,   std::placeholders::_1 ) );
           return true;
          }
          
          std::string
      58  NavigateToPoseNavigator::getDefaultBTFilepath(  
      59   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node )
          {
           std::string default_bt_xml_filename;
           auto node = parent_node.lock(   );
          
           if (  !node->has_parameter(  "default_nav_to_pose_bt_xml" ) ) {
           std::string pkg_share_dir =
           ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           node->declare_parameter<std::string>(  
           "default_nav_to_pose_bt_xml",  
           pkg_share_dir +
           "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" );
           }
          
           node->get_parameter(  "default_nav_to_pose_bt_xml",   default_bt_xml_filename );
          
           return default_bt_xml_filename;
          }
          
          bool
      79  NavigateToPoseNavigator::cleanup(   )
          {
           goal_sub_.reset(   );
           self_client_.reset(   );
           return true;
          }
          
          bool
      87  NavigateToPoseNavigator::goalReceived(  ActionT::Goal::ConstSharedPtr goal )
          {
           auto bt_xml_filename = goal->behavior_tree;
          
           if (  !bt_action_server_->loadBehaviorTree(  bt_xml_filename ) ) {
           RCLCPP_ERROR(  
           logger_,   "BT file not found: %s. Navigation canceled.",  
           bt_xml_filename.c_str(   ) );
           return false;
           }
          
           initializeGoalPose(  goal );
          
           return true;
          }
          
          void
     104  NavigateToPoseNavigator::goalCompleted(  
           typename ActionT::Result::SharedPtr /*result*/,  
           const nav2_behavior_tree::BtStatus /*final_bt_status*/ )
          {
          }
          
          void
     111  NavigateToPoseNavigator::onLoop(   )
          {
           // action server feedback (  pose,   duration of task,  
           // number of recoveries,   and distance remaining to goal )
           auto feedback_msg = std::make_shared<ActionT::Feedback>(   );
          
           geometry_msgs::msg::PoseStamped current_pose;
           nav2_util::getCurrentPose(  
           current_pose,   *feedback_utils_.tf,  
           feedback_utils_.global_frame,   feedback_utils_.robot_frame,  
           feedback_utils_.transform_tolerance );
          
           auto blackboard = bt_action_server_->getBlackboard(   );
          
           try {
           // Get current path points
           nav_msgs::msg::Path current_path;
           blackboard->get<nav_msgs::msg::Path>(  path_blackboard_id_,   current_path );
          
           // Find the closest pose to current pose on global path
           auto find_closest_pose_idx =
           [&current_pose,   &current_path](   ) {
           size_t closest_pose_idx = 0;
           double curr_min_dist = std::numeric_limits<double>::max(   );
           for (  size_t curr_idx = 0; curr_idx < current_path.poses.size(   ); ++curr_idx ) {
           double curr_dist = nav2_util::geometry_utils::euclidean_distance(  
           current_pose,   current_path.poses[curr_idx] );
           if (  curr_dist < curr_min_dist ) {
           curr_min_dist = curr_dist;
           closest_pose_idx = curr_idx;
           }
           }
           return closest_pose_idx;
           };
          
           // Calculate distance on the path
           double distance_remaining =
           nav2_util::geometry_utils::calculate_path_length(  current_path,   find_closest_pose_idx(   ) );
          
           // Default value for time remaining
           rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(  0.0 );
          
           // Get current speed
           geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(   );
           double current_linear_speed = std::hypot(  current_odom.linear.x,   current_odom.linear.y );
          
           // Calculate estimated time taken to goal if speed is higher than 1cm/s
           // and at least 10cm to go
           if (  (  std::abs(  current_linear_speed ) > 0.01 ) && (  distance_remaining > 0.1 ) ) {
           estimated_time_remaining =
           rclcpp::Duration::from_seconds(  distance_remaining / std::abs(  current_linear_speed ) );
           }
          
           feedback_msg->distance_remaining = distance_remaining;
           feedback_msg->estimated_time_remaining = estimated_time_remaining;
           } catch (  ... ) {
           // Ignore
           }
          
           int recovery_count = 0;
           blackboard->get<int>(  "number_recoveries",   recovery_count );
           feedback_msg->number_of_recoveries = recovery_count;
           feedback_msg->current_pose = current_pose;
           feedback_msg->navigation_time = clock_->now(   ) - start_time_;
          
           bt_action_server_->publishFeedback(  feedback_msg );
          }
          
          void
     180  NavigateToPoseNavigator::onPreempt(  ActionT::Goal::ConstSharedPtr goal )
          {
           RCLCPP_INFO(  logger_,   "Received goal preemption request" );
          
           if (  goal->behavior_tree == bt_action_server_->getCurrentBTFilename(   ) ||
           (  goal->behavior_tree.empty(   ) &&
           bt_action_server_->getCurrentBTFilename(   ) == bt_action_server_->getDefaultBTFilename(   ) ) )
           {
           // if pending goal requests the same BT as the current goal,   accept the pending goal
           // if pending goal has an empty behavior_tree field,   it requests the default BT file
           // accept the pending goal if the current goal is running the default BT file
           initializeGoalPose(  bt_action_server_->acceptPendingGoal(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "Preemption request was rejected since the requested BT XML file is not the same "
           "as the one that the current goal is executing. Preemption with a new BT is invalid "
           "since it would require cancellation of the previous goal instead of true preemption."
           "\nCancel the current goal and send a new action request if you want to use a "
           "different BT XML file. For now,   continuing to track the last goal until completion." );
           bt_action_server_->terminatePendingGoal(   );
           }
          }
          
          void
     205  NavigateToPoseNavigator::initializeGoalPose(  ActionT::Goal::ConstSharedPtr goal )
          {
           RCLCPP_INFO(  
           logger_,   "Begin navigating from current location to (  %.2f,   %.2f )",  
           goal->pose.pose.position.x,   goal->pose.pose.position.y );
          
           // Reset state for new action feedback
           start_time_ = clock_->now(   );
           auto blackboard = bt_action_server_->getBlackboard(   );
           blackboard->set<int>(  "number_recoveries",   0 ); // NOLINT
          
           // Update the goal pose on the blackboard
           blackboard->set<geometry_msgs::msg::PoseStamped>(  goal_blackboard_id_,   goal->pose );
          }
          
          void
     221  NavigateToPoseNavigator::onGoalPoseReceived(  const geometry_msgs::msg::PoseStamped::SharedPtr pose )
          {
           ActionT::Goal goal;
           goal.pose = *pose;
           self_client_->async_send_goal(  goal );
          }
          
          } // namespace nav2_bt_navigator

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp

          // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2020 Shrijit Singh
          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
          #define NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <algorithm>
          
          #include "nav2_core/smoother.hpp"
          #include "nav2_constrained_smoother/smoother.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/odometry_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          
          namespace nav2_constrained_smoother
          {
          
          /**
           * @class nav2_constrained_smoother::ConstrainedSmoother
           * @brief Regulated pure pursuit controller plugin
           */
      39  class ConstrainedSmoother : public nav2_core::Smoother
          {
          public:
           /**
           * @brief Constructor for nav2_constrained_smoother::ConstrainedSmoother
           */
           ConstrainedSmoother(   ) = default;
          
           /**
           * @brief Destrructor for nav2_constrained_smoother::ConstrainedSmoother
           */
           ~ConstrainedSmoother(   ) override = default;
          
           /**
           * @brief Configure smoother parameters and member variables
           * @param parent WeakPtr to node
           * @param name Name of plugin
           * @param tf TF buffer
           * @param costmap_ros Costmap2DROS object of environment
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,  
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub ) override;
          
           /**
           * @brief Cleanup controller state machine
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate controller state machine
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate controller state machine
           */
           void deactivate(   ) override;
          
           /**
           * @brief Method to smooth given path
           *
           * @param path In-out path to be optimized
           * @param max_time Maximum duration smoothing should take
           * @return Smoothed path
           */
           bool smooth(  
           nav_msgs::msg::Path & path,  
           const rclcpp::Duration & max_time ) override;
          
          protected:
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::string plugin_name_;
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
           rclcpp::Logger logger_ {rclcpp::get_logger(  "ConstrainedSmoother" )};
          
           std::unique_ptr<nav2_constrained_smoother::Smoother> smoother_;
           SmootherParams smoother_params_;
           OptimizerParams optimizer_params_;
          };
          
          } // namespace nav2_constrained_smoother
          
          #endif // NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
          #define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <memory>
          #include <queue>
          #include <utility>
          #include <deque>
          #include <limits>
          #include <algorithm>
          
          #include "nav2_constrained_smoother/smoother_cost_function.hpp"
          #include "nav2_constrained_smoother/utils.hpp"
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          
          namespace nav2_constrained_smoother
          {
          
          /**
           * @class nav2_smac_planner::Smoother
           * @brief A Conjugate Gradient 2D path smoother implementation
           */
      42  class Smoother
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::Smoother
           */
      48   Smoother(   ) {}
          
           /**
           * @brief A destructor for nav2_smac_planner::Smoother
           */
      53   ~Smoother(   ) {}
          
           /**
           * @brief Initialization of the smoother
           * @param params OptimizerParam struct
           */
      59   void initialize(  const OptimizerParams params )
           {
           debug_ = params.debug;
          
           options_.linear_solver_type = params.solver_types.at(  params.linear_solver_type );
          
           options_.max_num_iterations = params.max_iterations;
          
           options_.function_tolerance = params.fn_tol;
           options_.gradient_tolerance = params.gradient_tol;
           options_.parameter_tolerance = params.param_tol;
          
           if (  debug_ ) {
           options_.minimizer_progress_to_stdout = true;
           options_.logging_type = ceres::LoggingType::PER_MINIMIZER_ITERATION;
           } else {
           options_.logging_type = ceres::SILENT;
           }
           }
          
           /**
           * @brief Smoother method
           * @param path Reference to path
           * @param start_dir Orientation of the first pose
           * @param end_dir Orientation of the last pose
           * @param costmap Pointer to minimal costmap
           * @param params parameters weights
           * @return If smoothing was successful
           */
      88   bool smooth(  
      89   std::vector<Eigen::Vector3d> & path,  
      90   const Eigen::Vector2d & start_dir,  
      91   const Eigen::Vector2d & end_dir,  
      92   const nav2_costmap_2d::Costmap2D * costmap,  
           const SmootherParams & params )
           {
           // Path has always at least 2 points
           if (  path.size(   ) < 2 ) {
           throw std::runtime_error(  "Constrained smoother: Path must have at least 2 points" );
           }
          
           options_.max_solver_time_in_seconds = params.max_time;
          
           ceres::Problem problem;
           std::vector<Eigen::Vector3d> path_optim;
           std::vector<bool> optimized;
           if (  buildProblem(  path,   costmap,   params,   problem,   path_optim,   optimized ) ) {
           // solve the problem
           ceres::Solver::Summary summary;
           ceres::Solve(  options_,   &problem,   &summary );
           if (  debug_ ) {
           RCLCPP_INFO(  rclcpp::get_logger(  "smoother_server" ),   "%s",   summary.FullReport(   ).c_str(   ) );
           }
           if (  !summary.IsSolutionUsable(   ) || summary.initial_cost - summary.final_cost < 0.0 ) {
           return false;
           }
           } else {
           RCLCPP_INFO(  rclcpp::get_logger(  "smoother_server" ),   "Path too short to optimize" );
           }
          
           upsampleAndPopulate(  path_optim,   optimized,   start_dir,   end_dir,   params,   path );
          
           return true;
           }
          
          private:
           /**
           * @brief Build problem method
           * @param path Reference to path
           * @param costmap Pointer to costmap
           * @param params Smoother parameters
           * @param problem Output problem to solve
           * @param path_optim Output path on which the problem will be solved
           * @param optimized False for points skipped by downsampling
           * @return If there is a problem to solve
           */
     135   bool buildProblem(  
     136   const std::vector<Eigen::Vector3d> & path,  
     137   const nav2_costmap_2d::Costmap2D * costmap,  
           const SmootherParams & params,  
     139   ceres::Problem & problem,  
     140   std::vector<Eigen::Vector3d> & path_optim,  
     141   std::vector<bool> & optimized )
           {
           // Create costmap grid
           costmap_grid_ = std::make_shared<ceres::Grid2D<u_char>>(  
           costmap->getCharMap(   ),   0,   costmap->getSizeInCellsY(   ),   0,   costmap->getSizeInCellsX(   ) );
           auto costmap_interpolator = std::make_shared<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(  
           *costmap_grid_ );
          
           // Create residual blocks
           const double cusp_half_length = params.cusp_zone_length / 2;
           ceres::LossFunction * loss_function = NULL;
           path_optim = path;
           optimized = std::vector<bool>(  path.size(   ) );
           optimized[0] = true;
           int prelast_i = -1;
           int last_i = 0;
           double last_direction = path_optim[0][2];
           bool last_was_cusp = false;
           bool last_is_reversing = false;
           std::deque<std::pair<double,   SmootherCostFunction *>> potential_cusp_funcs;
           double last_segment_len = EPSILON;
           double potential_cusp_funcs_len = 0;
           double len_since_cusp = std::numeric_limits<double>::infinity(   );
          
           for (  size_t i = 1; i < path_optim.size(   ); i++ ) {
           auto & pt = path_optim[i];
           bool is_cusp = false;
           if (  i != path_optim.size(   ) - 1 ) {
           is_cusp = pt[2] * last_direction < 0;
           last_direction = pt[2];
          
           // skip to downsample if can be skipped (  no forward/reverse direction change )
           if (  !is_cusp &&
           i > (  params.keep_start_orientation ? 1 : 0 ) &&
           i < path_optim.size(   ) - (  params.keep_goal_orientation ? 2 : 1 ) &&
           static_cast<int>(  i - last_i ) < params.path_downsampling_factor )
           {
           continue;
           }
           }
          
           // keep distance inequalities between poses
           // (  some might have been downsampled while others might not )
           double current_segment_len = (  path_optim[i] - path_optim[last_i] ).block<2,   1>(  0,   0 ).norm(   );
          
           // forget cost functions which don't have chance to be part of a cusp zone
           potential_cusp_funcs_len += current_segment_len;
           while (  !potential_cusp_funcs.empty(   ) && potential_cusp_funcs_len > cusp_half_length ) {
           potential_cusp_funcs_len -= potential_cusp_funcs.front(   ).first;
           potential_cusp_funcs.pop_front(   );
           }
          
           // update cusp zone costmap weights
           if (  is_cusp ) {
           double len_to_cusp = current_segment_len;
           for (  int i = potential_cusp_funcs.size(   ) - 1; i >= 0; i-- ) {
           auto & f = potential_cusp_funcs[i];
           double new_weight =
           params.cusp_costmap_weight * (  1.0 - len_to_cusp / cusp_half_length ) +
           params.costmap_weight * len_to_cusp / cusp_half_length;
           if (  std::abs(  new_weight - params.cusp_costmap_weight ) <
           std::abs(  f.second->getCostmapWeight(   ) - params.cusp_costmap_weight ) )
           {
           f.second->setCostmapWeight(  new_weight );
           }
           len_to_cusp += f.first;
           }
           potential_cusp_funcs_len = 0;
           potential_cusp_funcs.clear(   );
           len_since_cusp = 0;
           }
          
           // add cost function
           optimized[i] = true;
           if (  prelast_i != -1 ) {
           double costmap_weight = params.costmap_weight;
           if (  len_since_cusp <= cusp_half_length ) {
           costmap_weight =
           params.cusp_costmap_weight * (  1.0 - len_since_cusp / cusp_half_length ) +
           params.costmap_weight * len_since_cusp / cusp_half_length;
           }
           SmootherCostFunction * cost_function = new SmootherCostFunction(  
           path[last_i].template block<2,   1>(  
           0,  
           0 ),  
           (  last_was_cusp ? -1 : 1 ) * last_segment_len / current_segment_len,  
           last_is_reversing,  
           costmap,  
           costmap_interpolator,  
           params,  
           costmap_weight
            );
           problem.AddResidualBlock(  
           cost_function->AutoDiff(   ),   loss_function,  
           path_optim[last_i].data(   ),   pt.data(   ),   path_optim[prelast_i].data(   ) );
          
           potential_cusp_funcs.emplace_back(  current_segment_len,   cost_function );
           }
          
           // shift current to last and last to pre-last
           last_was_cusp = is_cusp;
           last_is_reversing = last_direction < 0;
           prelast_i = last_i;
           last_i = i;
           len_since_cusp += current_segment_len;
           last_segment_len = std::max(  EPSILON,   current_segment_len );
           }
          
           int posesToOptimize = problem.NumParameterBlocks(   ) - 2; // minus start and goal
           if (  params.keep_goal_orientation ) {
           posesToOptimize -= 1; // minus goal orientation holder
           }
           if (  params.keep_start_orientation ) {
           posesToOptimize -= 1; // minus start orientation holder
           }
           if (  posesToOptimize <= 0 ) {
           return false; // nothing to optimize
           }
           // first two and last two points are constant (  to keep start and end direction )
           problem.SetParameterBlockConstant(  path_optim.front(   ).data(   ) );
           if (  params.keep_start_orientation ) {
           problem.SetParameterBlockConstant(  path_optim[1].data(   ) );
           }
           if (  params.keep_goal_orientation ) {
           problem.SetParameterBlockConstant(  path_optim[path_optim.size(   ) - 2].data(   ) );
           }
           problem.SetParameterBlockConstant(  path_optim.back(   ).data(   ) );
           return true;
           }
          
           /**
           * @brief Populate optimized points to path,   assigning orientations and upsampling poses using cubic bezier
           * @param path_optim Path with optimized points
           * @param optimized False for points skipped by downsampling
           * @param start_dir Orientation of the first pose
           * @param end_dir Orientation of the last pose
           * @param params Smoother parameters
           * @param path Output path with upsampled optimized points
           */
     280   void upsampleAndPopulate(  
     281   const std::vector<Eigen::Vector3d> & path_optim,  
     282   const std::vector<bool> & optimized,  
     283   const Eigen::Vector2d & start_dir,  
     284   const Eigen::Vector2d & end_dir,  
           const SmootherParams & params,  
     286   std::vector<Eigen::Vector3d> & path )
           {
           // Populate path,   assign orientations,   interpolate skipped/upsampled poses
           path.clear(   );
           if (  params.path_upsampling_factor > 1 ) {
           path.reserve(  params.path_upsampling_factor * (  path_optim.size(   ) - 1 ) + 1 );
           }
           int last_i = 0;
           int prelast_i = -1;
           Eigen::Vector2d prelast_dir;
           for (  int i = 1; i <= static_cast<int>(  path_optim.size(   ) ); i++ ) {
           if (  i == static_cast<int>(  path_optim.size(   ) ) || optimized[i] ) {
           if (  prelast_i != -1 ) {
           Eigen::Vector2d last_dir;
           auto & prelast = path_optim[prelast_i];
           auto & last = path_optim[last_i];
          
           // Compute orientation of last
           if (  i < static_cast<int>(  path_optim.size(   ) ) ) {
           auto & current = path_optim[i];
           Eigen::Vector2d tangent_dir = tangentDir<double>(  
           prelast.block<2,   1>(  0,   0 ),  
           last.block<2,   1>(  0,   0 ),  
           current.block<2,   1>(  0,   0 ),  
           prelast[2] * last[2] < 0 );
          
           last_dir =
           tangent_dir.dot(  (  current - last ).block<2,   1>(  0,   0 ) * last[2] ) >= 0 ?
           tangent_dir :
           -tangent_dir;
           last_dir.normalize(   );
           } else if (  params.keep_goal_orientation ) {
           last_dir = end_dir;
           } else {
           last_dir = (  last - prelast ).block<2,   1>(  0,   0 ) * last[2];
           last_dir.normalize(   );
           }
           double last_angle = atan2(  last_dir[1],   last_dir[0] );
          
           // Interpolate poses between prelast and last
           int interp_cnt = (  last_i - prelast_i ) * params.path_upsampling_factor - 1;
           if (  interp_cnt > 0 ) {
           Eigen::Vector2d last_pt = last.block<2,   1>(  0,   0 );
           Eigen::Vector2d prelast_pt = prelast.block<2,   1>(  0,   0 );
           double dist = (  last_pt - prelast_pt ).norm(   );
           Eigen::Vector2d pt1 = prelast_pt + prelast_dir * dist * 0.4 * prelast[2];
           Eigen::Vector2d pt2 = last_pt - last_dir * dist * 0.4 * prelast[2];
           for (  int j = 1; j <= interp_cnt; j++ ) {
           double interp = j / static_cast<double>(  interp_cnt + 1 );
           Eigen::Vector2d pt = cubicBezier(  prelast_pt,   pt1,   pt2,   last_pt,   interp );
           path.emplace_back(  pt[0],   pt[1],   0.0 );
           }
           }
           path.emplace_back(  last[0],   last[1],   last_angle );
          
           // Assign orientations to interpolated points
           for (  size_t j = path.size(   ) - 1 - interp_cnt; j < path.size(   ) - 1; j++ ) {
           Eigen::Vector2d tangent_dir = tangentDir<double>(  
           path[j - 1].block<2,   1>(  0,   0 ),  
           path[j].block<2,   1>(  0,   0 ),  
           path[j + 1].block<2,   1>(  0,   0 ),  
           false );
           tangent_dir =
           tangent_dir.dot(  (  path[j + 1] - path[j] ).block<2,   1>(  0,   0 ) * prelast[2] ) >= 0 ?
           tangent_dir :
           -tangent_dir;
           path[j][2] = atan2(  tangent_dir[1],   tangent_dir[0] );
           }
          
           prelast_dir = last_dir;
           } else { // start pose
           auto & start = path_optim[0];
           Eigen::Vector2d dir = params.keep_start_orientation ?
           start_dir :
           (  (  path_optim[i] - start ).block<2,   1>(  0,   0 ) * start[2] ).normalized(   );
           path.emplace_back(  start[0],   start[1],   atan2(  dir[1],   dir[0] ) );
           prelast_dir = dir;
           }
           prelast_i = last_i;
           last_i = i;
           }
           }
           }
          
           /*
           Piecewise cubic bezier curve as defined by Adobe in Postscript
           The two end points are pt0 and pt3
           Their associated control points are pt1 and pt2
           */
     375   static Eigen::Vector2d cubicBezier(  
     376   Eigen::Vector2d & pt0,   Eigen::Vector2d & pt1,  
     377   Eigen::Vector2d & pt2,   Eigen::Vector2d & pt3,   double mu )
           {
           Eigen::Vector2d a,   b,   c,   pt;
          
           c[0] = 3 * (  pt1[0] - pt0[0] );
           c[1] = 3 * (  pt1[1] - pt0[1] );
           b[0] = 3 * (  pt2[0] - pt1[0] ) - c[0];
           b[1] = 3 * (  pt2[1] - pt1[1] ) - c[1];
           a[0] = pt3[0] - pt0[0] - c[0] - b[0];
           a[1] = pt3[1] - pt0[1] - c[1] - b[1];
          
           pt[0] = a[0] * mu * mu * mu + b[0] * mu * mu + c[0] * mu + pt0[0];
           pt[1] = a[1] * mu * mu * mu + b[1] * mu * mu + c[1] * mu + pt0[1];
          
           return pt;
           }
          
     394   bool debug_;
     395   ceres::Solver::Options options_;
           std::shared_ptr<ceres::Grid2D<u_char>> costmap_grid_;
          };
          
          } // namespace nav2_constrained_smoother
          
          #endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp

          // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
          #define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <unordered_map>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "ceres/ceres.h"
          #include "ceres/cubic_interpolation.h"
          #include "Eigen/Core"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_constrained_smoother/options.hpp"
          #include "nav2_constrained_smoother/utils.hpp"
          
          namespace nav2_constrained_smoother
          {
          
          /**
           * @struct nav2_constrained_smoother::SmootherCostFunction
           * @brief Cost function for path smoothing with multiple terms
           * including curvature,   smoothness,   distance from original and obstacle avoidance.
           */
      42  class SmootherCostFunction
          {
          public:
           /**
           * @brief A constructor for nav2_constrained_smoother::SmootherCostFunction
           * @param original_path Original position of the path node
           * @param next_to_last_length_ratio Ratio of next path segment compared to previous.
           * Negative if one of them represents reversing motion.
           * @param reversing Whether the path segment after this node represents reversing motion.
           * @param costmap A costmap to get values for collision and obstacle avoidance
           * @param params Optimization weights and parameters
           * @param costmap_weight Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight
           */
      55   SmootherCostFunction(  
           const Eigen::Vector2d & original_pos,  
           double next_to_last_length_ratio,  
           bool reversing,  
           const nav2_costmap_2d::Costmap2D * costmap,  
           const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,  
           const SmootherParams & params,  
           double costmap_weight )
           : original_pos_(  original_pos ),  
           next_to_last_length_ratio_(  next_to_last_length_ratio ),  
           reversing_(  reversing ),  
           params_(  params ),  
           costmap_weight_(  costmap_weight ),  
           costmap_origin_(  costmap->getOriginX(   ),   costmap->getOriginY(   ) ),  
           costmap_resolution_(  costmap->getResolution(   ) ),  
           costmap_interpolator_(  costmap_interpolator )
           {
           }
          
      74   ceres::CostFunction * AutoDiff(   )
           {
           return new ceres::AutoDiffCostFunction<SmootherCostFunction,   4,   2,   2,   2>(  this );
           }
          
      79   void setCostmapWeight(  double costmap_weight )
           {
           costmap_weight_ = costmap_weight;
           }
          
      84   double getCostmapWeight(   )
           {
           return costmap_weight_;
           }
          
           /**
           * @brief Smoother cost function evaluation
           * @param pt X,   Y coords of current point
           * @param pt_next X,   Y coords of next point
           * @param pt_prev X,   Y coords of previous point
           * @param pt_residual array of output residuals (  smoothing,   curvature,   distance,   cost )
           * @return if successful in computing values
           */
           template<typename T>
           bool operator(   )(  
           const T * const pt,   const T * const pt_next,   const T * const pt_prev,  
           T * pt_residual ) const
           {
           Eigen::Map<const Eigen::Matrix<T,   2,   1>> xi(  pt );
           Eigen::Map<const Eigen::Matrix<T,   2,   1>> xi_next(  pt_next );
           Eigen::Map<const Eigen::Matrix<T,   2,   1>> xi_prev(  pt_prev );
           Eigen::Map<Eigen::Matrix<T,   4,   1>> residual(  pt_residual );
           residual.setZero(   );
          
           // compute cost
           addSmoothingResidual<T>(  params_.smooth_weight,   xi,   xi_next,   xi_prev,   residual[0] );
           addCurvatureResidual<T>(  params_.curvature_weight,   xi,   xi_next,   xi_prev,   residual[1] );
           addDistanceResidual<T>(  
           params_.distance_weight,   xi,  
           original_pos_.template cast<T>(   ),   residual[2] );
           addCostResidual<T>(  costmap_weight_,   xi,   xi_next,   xi_prev,   residual[3] );
          
           return true;
           }
          
          protected:
           /**
           * @brief Cost function term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt_next Point Xi+1 for calculating Xi's cost
           * @param pt_prev Point Xi-1 for calculating Xi's cost
           * @param r Residual (  cost ) of term
           */
           template<typename T>
           inline void addSmoothingResidual(  
           const double & weight,  
           const Eigen::Matrix<T,   2,   1> & pt,  
           const Eigen::Matrix<T,   2,   1> & pt_next,  
           const Eigen::Matrix<T,   2,   1> & pt_prev,  
           T & r ) const
           {
           Eigen::Matrix<T,   2,   1> d_next = pt_next - pt;
           Eigen::Matrix<T,   2,   1> d_prev = pt - pt_prev;
           Eigen::Matrix<T,   2,   1> d_diff = next_to_last_length_ratio_ * d_next - d_prev;
     139   r += (  T )weight * d_diff.dot(  d_diff ); // objective function value
           }
          
           /**
           * @brief Cost function term for maximum curved paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt_next Point Xi+1 for calculating Xi's cost
           * @param pt_prev Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           * @param r Residual (  cost ) of term
           */
           template<typename T>
           inline void addCurvatureResidual(  
           const double & weight,  
           const Eigen::Matrix<T,   2,   1> & pt,  
           const Eigen::Matrix<T,   2,   1> & pt_next,  
           const Eigen::Matrix<T,   2,   1> & pt_prev,  
           T & r ) const
           {
           Eigen::Matrix<T,   2,   1> center = arcCenter(  
           pt_prev,   pt,   pt_next,  
           next_to_last_length_ratio_ < 0 );
           if (  ceres::IsInfinite(  center[0] ) ) {
           return;
           }
           T turning_rad = (  pt - center ).norm(   );
           T ki_minus_kmax = (  T )1.0 / turning_rad - params_.max_curvature;
          
           if (  ki_minus_kmax <= (  T )EPSILON ) {
           return;
           }
          
           r += (  T )weight * ki_minus_kmax * ki_minus_kmax; // objective function value
           }
          
           /**
           * @brief Cost function derivative term for steering away changes in pose
           * @param weight Weight to apply to function
           * @param xi Point Xi for evaluation
           * @param xi_original original point Xi for evaluation
           * @param r Residual (  cost ) of term
           */
           template<typename T>
           inline void addDistanceResidual(  
           const double & weight,  
           const Eigen::Matrix<T,   2,   1> & xi,  
           const Eigen::Matrix<T,   2,   1> & xi_original,  
           T & r ) const
           {
           r += (  T )weight * (  xi - xi_original ).squaredNorm(   ); // objective function value
           }
          
           /**
           * @brief Cost function term for steering away from costs
           * @param weight Weight to apply to function
           * @param value Point Xi's cost'
           * @param params computed values to reduce overhead
           * @param r Residual (  cost ) of term
           */
           template<typename T>
           inline void addCostResidual(  
           const double & weight,  
           const Eigen::Matrix<T,   2,   1> & pt,  
           const Eigen::Matrix<T,   2,   1> & pt_next,  
           const Eigen::Matrix<T,   2,   1> & pt_prev,  
           T & r ) const
           {
           if (  params_.cost_check_points.empty(   ) ) {
           Eigen::Matrix<T,   2,   1> interp_pos =
           (  pt - costmap_origin_.template cast<T>(   ) ) / (  T )costmap_resolution_;
           T value;
           costmap_interpolator_->Evaluate(  interp_pos[1] - (  T )0.5,   interp_pos[0] - (  T )0.5,   &value );
           r += (  T )weight * value * value; // objective function value
           } else {
           Eigen::Matrix<T,   2,   1> dir = tangentDir(  
           pt_prev,   pt,   pt_next,  
           next_to_last_length_ratio_ < 0 );
           dir.normalize(   );
           if (  (  (  pt_next - pt ).dot(  dir ) < (  T )0 ) != reversing_ ) {
           dir = -dir;
           }
           Eigen::Matrix<T,   3,   3> transform;
           transform << dir[0],   -dir[1],   pt[0],  
           dir[1],   dir[0],   pt[1],  
           (  T )0,   (  T )0,   (  T )1;
           for (  size_t i = 0; i < params_.cost_check_points.size(   ); i += 3 ) {
           Eigen::Matrix<T,   3,   1> ccpt(  (  T )params_.cost_check_points[i],  
           (  T )params_.cost_check_points[i + 1],   (  T )1 );
           auto ccpt_world = (  transform * ccpt ).template block<2,   1>(  0,   0 );
           Eigen::Matrix<T,   2,  
           1> interp_pos = (  ccpt_world - costmap_origin_.template cast<T>(   ) ) /
           (  T )costmap_resolution_;
           T value;
           costmap_interpolator_->Evaluate(  interp_pos[1] - (  T )0.5,   interp_pos[0] - (  T )0.5,   &value );
          
           r += (  T )weight * (  T )params_.cost_check_points[i + 2] * value * value;
           }
           }
           }
          
           const Eigen::Vector2d original_pos_;
           double next_to_last_length_ratio_;
           bool reversing_;
           SmootherParams params_;
           double costmap_weight_;
           Eigen::Vector2d costmap_origin_;
           double costmap_resolution_;
           std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> costmap_interpolator_;
          };
          
          } // namespace nav2_constrained_smoother
          
          #endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
          #define NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
          
          #include <limits>
          #include "Eigen/Core"
          
          #define EPSILON 0.0001
          
          namespace nav2_constrained_smoother
          {
          
          /**
           * @brief Center of an arc between three points
           * @param pt_prev Starting point of the arc
           * @param pt Mid point of the arc
           * @param pt_next Last point of the arc
           * @param is_cusp True if pt is a cusp point
           * @result position of the center or Vector2(  inf,   inf ) for straight lines and 180 deg turns
           */
          template<typename T>
      35  inline Eigen::Matrix<T,   2,   1> arcCenter(  
           Eigen::Matrix<T,   2,   1> pt_prev,  
           Eigen::Matrix<T,   2,   1> pt,  
           Eigen::Matrix<T,   2,   1> pt_next,  
           bool is_cusp )
          {
           Eigen::Matrix<T,   2,   1> d1 = pt - pt_prev;
           Eigen::Matrix<T,   2,   1> d2 = pt_next - pt;
          
           if (  is_cusp ) {
           d2 = -d2;
           pt_next = pt + d2;
           }
          
           T det = d1[0] * d2[1] - d1[1] * d2[0];
           if (  ceres::abs(  det ) < (  T )1e-4 ) { // straight line
           return Eigen::Matrix<T,   2,   1>(  
           (  T )std::numeric_limits<double>::infinity(   ),   (  T )std::numeric_limits<double>::infinity(   ) );
           }
          
           // circle center is at the intersection of mirror axes of the segments:
           // http://paulbourke.net/geometry/circlesphere/
           // line intersection:
           // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
           Eigen::Matrix<T,   2,   1> mid1 = (  pt_prev + pt ) / (  T )2;
           Eigen::Matrix<T,   2,   1> mid2 = (  pt + pt_next ) / (  T )2;
           Eigen::Matrix<T,   2,   1> n1(  -d1[1],   d1[0] );
           Eigen::Matrix<T,   2,   1> n2(  -d2[1],   d2[0] );
           T det1 = (  mid1[0] + n1[0] ) * mid1[1] - (  mid1[1] + n1[1] ) * mid1[0];
           T det2 = (  mid2[0] + n2[0] ) * mid2[1] - (  mid2[1] + n2[1] ) * mid2[0];
           Eigen::Matrix<T,   2,   1> center(  (  det1 * n2[0] - det2 * n1[0] ) / det,  
           (  det1 * n2[1] - det2 * n1[1] ) / det );
           return center;
          }
          
          /**
           * @brief Direction of a line which contains pt and is tangential to arc
           * between pt_prev,   pt,   pt_next
           * @param pt_prev Starting point of the arc
           * @param pt Mid point of the arc,   lying on the tangential line
           * @param pt_next Last point of the arc
           * @param is_cusp True if pt is a cusp point
           * @result Tangential line direction.
           * Note: the sign of tangentDir is undefined here,   should be assigned in post-process
           * depending on movement direction. Also,   for speed reasons,   direction vector is not normalized.
           */
          template<typename T>
      82  inline Eigen::Matrix<T,   2,   1> tangentDir(  
           Eigen::Matrix<T,   2,   1> pt_prev,  
           Eigen::Matrix<T,   2,   1> pt,  
           Eigen::Matrix<T,   2,   1> pt_next,  
           bool is_cusp )
          {
           Eigen::Matrix<T,   2,   1> center = arcCenter(  pt_prev,   pt,   pt_next,   is_cusp );
           if (  ceres::IsInfinite(  center[0] ) ) { // straight line
           Eigen::Matrix<T,   2,   1> d1 = pt - pt_prev;
           Eigen::Matrix<T,   2,   1> d2 = pt_next - pt;
          
           if (  is_cusp ) {
           d2 = -d2;
           pt_next = pt + d2;
           }
          
           Eigen::Matrix<T,   2,   1> result(  pt_next[0] - pt_prev[0],   pt_next[1] - pt_prev[1] );
           if (  result[0] == 0.0 && result[1] == 0.0 ) { // a very rare edge situation
           return Eigen::Matrix<T,   2,   1>(  d1[1],   -d1[0] );
           }
           return result;
           }
          
           // tangent is prependicular to (  pt - center )
           // Note: not determining + or - direction here,   this should be handled at the caller side
           return Eigen::Matrix<T,   2,   1>(  center[1] - pt[1],   pt[0] - center[0] );
          }
          
          } // namespace nav2_constrained_smoother
          
          #endif // NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_

navigation2/nav2_constrained_smoother/src/constrained_smoother.cpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2020 Shrijit Singh
          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <algorithm>
          #include <string>
          #include <memory>
          #include <utility>
          #include <vector>
          
          #include "nav2_constrained_smoother/constrained_smoother.hpp"
          #include "nav2_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          #include "tf2/utils.h"
          
          using nav2_util::declare_parameter_if_not_declared;
          using nav2_util::geometry_utils::euclidean_distance;
          using namespace nav2_costmap_2d; // NOLINT
          
          namespace nav2_constrained_smoother
          {
          
      41  void ConstrainedSmoother::configure(  
      42   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      43   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      44   std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,  
      45   std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> )
          {
           auto node = parent.lock(   );
           if (  !node ) {
           throw std::runtime_error(  "Unable to lock node!" );
           }
          
           costmap_sub_ = costmap_sub;
           tf_ = tf;
           plugin_name_ = name;
           logger_ = node->get_logger(   );
          
           smoother_ = std::make_unique<nav2_constrained_smoother::Smoother>(   );
           optimizer_params_.get(  node.get(   ),   name );
           smoother_params_.get(  node.get(   ),   name );
           smoother_->initialize(  optimizer_params_ );
          }
          
      63  void ConstrainedSmoother::cleanup(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Cleaning up smoother: %s of type"
           " nav2_constrained_smoother::ConstrainedSmoother",  
           plugin_name_.c_str(   ) );
          }
          
      72  void ConstrainedSmoother::activate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Activating smoother: %s of type "
           "nav2_constrained_smoother::ConstrainedSmoother",  
           plugin_name_.c_str(   ) );
          }
          
      81  void ConstrainedSmoother::deactivate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Deactivating smoother: %s of type "
           "nav2_constrained_smoother::ConstrainedSmoother",  
           plugin_name_.c_str(   ) );
          }
          
      90  bool ConstrainedSmoother::smooth(  nav_msgs::msg::Path & path,   const rclcpp::Duration & max_time )
          {
           if (  path.poses.size(   ) < 2 ) {
           return true;
           }
          
           // populate smoother input with (  x,   y,   forward/reverse dir )
           std::vector<Eigen::Vector3d> path_world;
           path_world.reserve(  path.poses.size(   ) );
           // smoother keeps record of start/end orientations so that it
           // can use them in the final path,   preventing degradation of these (  often important ) values
           Eigen::Vector2d start_dir;
           Eigen::Vector2d end_dir;
           for (  size_t i = 0; i < path.poses.size(   ); i++ ) {
           auto & pose = path.poses[i].pose;
           double angle = tf2::getYaw(  pose.orientation );
           Eigen::Vector2d orientation(  cos(  angle ),   sin(  angle ) );
           if (  i == path.poses.size(   ) - 1 ) {
           // Note: `reversing` indicates the direction of the segment after the point and
           // there is no segment after the last point. Most probably the value is irrelevant,   but
           // copying it from the last but one point,   just to make it defined...
           path_world.emplace_back(  pose.position.x,   pose.position.y,   path_world.back(   )[2] );
           end_dir = orientation;
           } else {
           auto & pos_next = path.poses[i + 1].pose.position;
           Eigen::Vector2d mvmt(  pos_next.x - pose.position.x,   pos_next.y - pose.position.y );
           // robot is considered reversing when angle between its orientation and movement direction
           // is more than 90 degrees (  i.e. dot product is less than 0 )
           bool reversing = smoother_params_.reversing_enabled && orientation.dot(  mvmt ) < 0;
           // we transform boolean value of "reversing" into sign of movement direction (  +1 or -1 )
           // to simplify further computations
           path_world.emplace_back(  pose.position.x,   pose.position.y,   reversing ? -1 : 1 );
           if (  i == 0 ) {
           start_dir = orientation;
           } else if (  i == 1 && !smoother_params_.keep_start_orientation ) {
           // overwrite start forward/reverse when orientation was set to be ignored
           // note: start_dir is overwritten inside Smoother::upsampleAndPopulate(   ) method
           path_world[0][2] = path_world.back(   )[2];
           }
           }
           }
          
           smoother_params_.max_time = max_time.seconds(   );
          
           // Smooth plan
           auto costmap = costmap_sub_->getCostmap(   );
           if (  !smoother_->smooth(  path_world,   start_dir,   end_dir,   costmap.get(   ),   smoother_params_ ) ) {
           RCLCPP_WARN(  
           logger_,  
           "%s: failed to smooth plan,   Ceres could not find a usable solution to optimize.",  
           plugin_name_.c_str(   ) );
           throw new nav2_core::PlannerException(  
           "Failed to smooth plan,   Ceres could not find a usable solution." );
           }
          
           // populate final path
           geometry_msgs::msg::PoseStamped pose;
           pose.header = path.poses.front(   ).header;
           path.poses.clear(   );
           path.poses.reserve(  path_world.size(   ) );
           for (  auto & pw : path_world ) {
           pose.pose.position.x = pw[0];
           pose.pose.position.y = pw[1];
           pose.pose.orientation.z = sin(  pw[2] / 2 );
           pose.pose.orientation.w = cos(  pw[2] / 2 );
          
           path.poses.push_back(  pose );
           }
          
           return true;
          }
          
          } // namespace nav2_constrained_smoother
          
          // Register this smoother as a nav2_core plugin
     165  PLUGINLIB_EXPORT_CLASS(  
           nav2_constrained_smoother::ConstrainedSmoother,  
           nav2_core::Smoother )

navigation2/nav2_constrained_smoother/test/test_constrained_smoother.cpp

          // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <chrono>
          #include <iostream>
          #include <future>
          #include <thread>
          #include <algorithm>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_ros/create_timer_ros.h"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_costmap_2d/inflation_layer.hpp"
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
          #include "angles/angles.h"
          
          #include "nav2_constrained_smoother/constrained_smoother.hpp"
          
          #include "geometry_msgs/msg/pose_array.hpp"
          
      37  class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
          {
          public:
      40   DummyCostmapSubscriber(  
      41   nav2_util::LifecycleNode::SharedPtr node,  
      42   const std::string & topic_name )
           : CostmapSubscriber(  node,   topic_name )
           {
           auto costmap = std::make_shared<nav2_msgs::msg::Costmap>(   );
           costmap->metadata.size_x = 100;
           costmap->metadata.size_y = 100;
           costmap->metadata.resolution = 0.1;
           costmap->metadata.origin.position.x = -5.0;
           costmap->metadata.origin.position.y = -5.0;
          
           costmap->data.resize(  costmap->metadata.size_x * costmap->metadata.size_y,   0 );
          
           // create an obstacle in rect (  1.0,   -1.0 ) -> (  3.0,   -2.0 )
           // with inflation of radius 2.0
           double cost_scaling_factor = 1.6;
           double inscribed_radius = 0.3;
           for (  int i = 10; i < 60; ++i ) {
           for (  int j = 40; j < 100; ++j ) {
           int dist_x = std::max(  0,   std::max(  60 - j,   j - 80 ) );
           int dist_y = std::max(  0,   std::max(  30 - i,   i - 40 ) );
           double dist = sqrt(  dist_x * dist_x + dist_y * dist_y );
           unsigned char cost;
           if (  dist == 0 ) {
           cost = nav2_costmap_2d::LETHAL_OBSTACLE;
           } else if (  dist < inscribed_radius ) {
           cost = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
           } else {
           double factor =
           exp(  
           -1.0 * cost_scaling_factor * (  dist * costmap->metadata.resolution - inscribed_radius ) );
           cost =
           static_cast<unsigned char>(  (  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1 ) * factor );
           }
           costmap->data[i * costmap->metadata.size_x + j] = cost;
           }
           }
          
           setCostmap(  costmap );
           }
          
      82   void setCostmap(  nav2_msgs::msg::Costmap::SharedPtr msg )
           {
           costmap_msg_ = msg;
           costmap_received_ = true;
           }
          };
          
      89  geometry_msgs::msg::Point pointMsg(  double x,   double y )
          {
           geometry_msgs::msg::Point point;
           point.x = x;
           point.y = y;
           return point;
          }
          
      97  class SmootherTest : public ::testing::Test
          {
          protected:
     100   SmootherTest(   ) {SetUp(   );}
     101   ~SmootherTest(   ) {}
          
     103   void SetUp(   )
           {
           node_lifecycle_ =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  
           "ConstrainedSmootherTestNode",   rclcpp::NodeOptions(   ) );
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_lifecycle_->get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           node_lifecycle_->get_node_base_interface(   ),  
           node_lifecycle_->get_node_timers_interface(   ) );
           tf_buffer_->setCreateTimerInterface(  timer_interface );
          
           costmap_sub_ =
           std::make_shared<DummyCostmapSubscriber>(  
           node_lifecycle_,   "costmap_topic" );
          
           path_poses_pub_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(  
           "/plan_poses_optimized",   100 );
           path_poses_pub_cmp_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(  
           "/plan_poses_optimized_cmp",   100 );
           path_poses_pub_orig_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(  
           "/plan_poses_original",   100 );
           costmap_pub_ = std::make_shared<nav2_costmap_2d::Costmap2DPublisher>(  
           node_lifecycle_,  
           costmap_sub_->getCostmap(   ).get(   ),   "map",   "/costmap",   true );
          
           node_lifecycle_->configure(   );
           node_lifecycle_->activate(   );
           path_poses_pub_->on_activate(   );
           path_poses_pub_cmp_->on_activate(   );
           path_poses_pub_orig_->on_activate(   );
           costmap_pub_->on_activate(   );
          
          
           smoother_ = std::make_shared<nav2_constrained_smoother::ConstrainedSmoother>(   );
          
           smoother_->configure(  
           node_lifecycle_,   "SmoothPath",   tf_buffer_,   costmap_sub_,  
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>(   ) );
           smoother_->activate(   );
          
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_smooth",   15000.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.minimum_turning_radius",   0.4 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   30.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_dist",   0.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.cusp_zone_length",   -1.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost_cusp_multiplier",   1.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_downsampling_factor",   1 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_upsampling_factor",   1 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.reversing_enabled",   true ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.keep_start_orientation",   true ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.keep_goal_orientation",   true ) );
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  "SmoothPath.optimizer.linear_solver_type",   "SPARSE_NORMAL_CHOLESKY" ) );
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(   ) ) );
           reloadParams(   );
           }
          
           void TearDown(   ) override
           {
           smoother_->deactivate(   );
           smoother_->cleanup(   );
           path_poses_pub_->on_deactivate(   );
           path_poses_pub_cmp_->on_deactivate(   );
           path_poses_pub_orig_->on_deactivate(   );
           costmap_pub_->on_deactivate(   );
           node_lifecycle_->deactivate(   );
           }
          
           void reloadParams(   )
           {
           smoother_->deactivate(   );
     179   smoother_->cleanup(   );
     180   smoother_->configure(  
           node_lifecycle_,   "SmoothPath",   tf_buffer_,   costmap_sub_,  
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>(   ) );
           smoother_->activate(   );
           }
          
           bool smoothPath(  
           const std::vector<Eigen::Vector3d> & input,   std::vector<Eigen::Vector3d> & output,  
           bool publish = false,   bool cmp = false )
           {
           nav_msgs::msg::Path path;
           path.poses.reserve(  input.size(   ) );
           for (  auto & xya : input ) {
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = xya.x(   );
           pose.pose.position.y = xya.y(   );
           pose.pose.position.z = 0;
           pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  xya.z(   ) );
           path.poses.push_back(  pose );
           }
          
           if (  publish && !path.poses.empty(   ) ) {
           geometry_msgs::msg::PoseArray poses;
           poses.header.frame_id = "map";
           poses.header.stamp = node_lifecycle_->get_clock(   )->now(   );
           for (  auto & p : path.poses ) {
           poses.poses.push_back(  p.pose );
           }
           path_poses_pub_orig_->publish(  poses );
           costmap_pub_->publishCostmap(   );
           }
          
           bool result = smoother_->smooth(  path,   rclcpp::Duration::from_seconds(  10.0 ) );
          
           if (  publish && !path.poses.empty(   ) ) {
           geometry_msgs::msg::PoseArray poses;
           poses.header.frame_id = "map";
           poses.header.stamp = node_lifecycle_->get_clock(   )->now(   );
           for (  auto & p : path.poses ) {
           poses.poses.push_back(  p.pose );
           }
           auto & pub = cmp ? path_poses_pub_cmp_ : path_poses_pub_;
           pub->publish(  poses );
           }
          
           output.clear(   );
           output.reserve(  path.poses.size(   ) );
           for (  auto & pose : path.poses ) {
           Eigen::Vector3d xya;
           xya.x(   ) = pose.pose.position.x;
           xya.y(   ) = pose.pose.position.y;
           tf2::Quaternion q;
           tf2::fromMsg(  pose.pose.orientation,   q );
           xya.z(   ) = q.getAngle(   );
           output.push_back(  xya );
           }
           return result;
           }
          
           typedef std::function<double (  int i,   const Eigen::Vector3d & prev_p,   const Eigen::Vector3d & p,  
           const Eigen::Vector3d & next_p )> QualityCriterion3;
           typedef std::function<double (  int i,   const Eigen::Vector3d & prev_p,  
           const Eigen::Vector3d & p )> QualityCriterion2;
           typedef std::function<double (  int i,   const Eigen::Vector3d & p )> QualityCriterion1;
           /**
           * @brief Path improvement assessment method
           * @param input Smoother input path
           * @param output Smoother output path
           * @param criterion Criterion of path quality. Total path quality = sqrt(  sum(  criterion(  data[i] )^2 )/count(  data ) )
           * @return Percentage of improvement (  relative to input path quality )
           */
           double assessPathImprovement(  
           const std::vector<Eigen::Vector3d> & input,  
           const std::vector<Eigen::Vector3d> & output,  
           const QualityCriterion3 & criterion,  
           const QualityCriterion3 * criterion_out = nullptr )
           {
           if (  !criterion_out ) {
           criterion_out = &criterion;
           }
           double total_input_crit = 0.0;
           for (  size_t i = 1; i < input.size(   ) - 1; i++ ) {
           double input_crit = criterion(  i,   input[i - 1],   input[i],   input[i + 1] );
           total_input_crit += input_crit * input_crit;
           }
           total_input_crit = sqrt(  total_input_crit / (  input.size(   ) - 2 ) );
          
           double total_output_crit = 0.0;
           for (  size_t i = 1; i < output.size(   ) - 1; i++ ) {
           double output_crit = (  *criterion_out )(  i,   output[i - 1],   output[i],   output[i + 1] );
           total_output_crit += output_crit * output_crit;
           }
           total_output_crit = sqrt(  total_output_crit / (  input.size(   ) - 2 ) );
          
           return (  1.0 - total_output_crit / total_input_crit ) * 100;
           }
          
           /**
           * @brief Path improvement assessment method
           * @param input Smoother input path
           * @param output Smoother output path
           * @param criterion Criterion of path quality. Total path quality = sqrt(  sum(  criterion(  data[i] )^2 )/count(  data ) )
           * @return Percentage of improvement (  relative to input path quality )
           */
           double assessPathImprovement(  
           const std::vector<Eigen::Vector3d> & input,  
           const std::vector<Eigen::Vector3d> & output,  
           const QualityCriterion2 & criterion,  
           const QualityCriterion2 * criterion_out = nullptr )
           {
           if (  !criterion_out ) {
           criterion_out = &criterion;
           }
           double total_input_crit = 0.0;
           for (  size_t i = 1; i < input.size(   ); i++ ) {
           double input_crit = criterion(  i,   input[i - 1],   input[i] );
           total_input_crit += input_crit * input_crit;
           }
           total_input_crit = sqrt(  total_input_crit / (  input.size(   ) - 1 ) );
          
           double total_output_crit = 0.0;
           for (  size_t i = 1; i < output.size(   ); i++ ) {
           double output_crit = (  *criterion_out )(  i,   output[i - 1],   output[i] );
           total_output_crit += output_crit * output_crit;
           }
           total_output_crit = sqrt(  total_output_crit / (  output.size(   ) - 1 ) );
          
           return (  1.0 - total_output_crit / total_input_crit ) * 100;
           }
          
           /**
           * @brief Path improvement assessment method
           * @param input Smoother input path
           * @param output Smoother output path
           * @param criterion Criterion of path quality. Total path quality = sqrt(  sum(  criterion(  data[i] )^2 )/count(  data ) )
           * @return Percentage of improvement (  relative to input path quality )
           */
           double assessPathImprovement(  
           const std::vector<Eigen::Vector3d> & input,  
           const std::vector<Eigen::Vector3d> & output,  
           const QualityCriterion1 & criterion,  
           const QualityCriterion1 * criterion_out = nullptr )
           {
           if (  !criterion_out ) {
           criterion_out = &criterion;
           }
           double total_input_crit = 0.0;
           for (  size_t i = 0; i < input.size(   ); i++ ) {
           double input_crit = criterion(  i,   input[i] );
           total_input_crit += input_crit * input_crit;
           }
           total_input_crit = sqrt(  total_input_crit / input.size(   ) );
          
           double total_output_crit = 0.0;
           for (  size_t i = 0; i < output.size(   ); i++ ) {
           double output_crit = (  *criterion_out )(  i,   output[i] );
           total_output_crit += output_crit * output_crit;
           }
           total_output_crit = sqrt(  total_output_crit / output.size(   ) );
          
           return (  1.0 - total_output_crit / total_input_crit ) * 100;
           }
          
           /**
           * @brief Worst pose improvement assessment method
           * @param input Smoother input path
           * @param output Smoother output path
           * @param criterion Criterion of path quality. Total path quality = max(  criterion(  data[i] ) )
           * @return Percentage of improvement (  relative to input path quality )
           */
           double assessWorstPoseImprovement(  
           const std::vector<Eigen::Vector3d> & input,  
           const std::vector<Eigen::Vector3d> & output,  
           const QualityCriterion1 & criterion )
           {
           double max_input_crit = 0.0;
           for (  size_t i = 0; i < input.size(   ); i++ ) {
           double input_crit = criterion(  i,   input[i] );
           max_input_crit = std::max(  max_input_crit,   input_crit );
           }
          
           double max_output_crit = 0.0;
           for (  size_t i = 0; i < output.size(   ); i++ ) {
           double output_crit = criterion(  i,   output[i] );
           max_output_crit = std::max(  max_output_crit,   output_crit );
           }
          
           return (  1.0 - max_output_crit / max_input_crit ) * 100;
           }
          
           std::vector<Eigen::Vector3d> zigZaggedPath(  
           const std::vector<Eigen::Vector3d> & input,  
           double offset )
           {
           auto output = input;
           for (  size_t i = 1; i < input.size(   ) - 1; i++ ) {
           // add offset prependicular to path
           Eigen::Vector2d direction =
           (  input[i + 1].block<2,   1>(  0,   0 ) - input[i - 1].block<2,   1>(  0,   0 ) ).normalized(   );
           output[i].block<2,   1>(  
           0,  
           0 ) += Eigen::Vector2d(  direction[1],   -direction[0] ) * offset * (  i % 2 == 0 ? 1.0 : -1.0 );
           }
           return output;
           }
          
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
           std::shared_ptr<nav2_constrained_smoother::ConstrainedSmoother> smoother_;
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
          
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
           path_poses_pub_orig_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr path_poses_pub_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
           path_poses_pub_cmp_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DPublisher> costmap_pub_;
          
           int cusp_i_ = -1;
           QualityCriterion3 mvmt_smoothness_criterion_ =
           [this](  int i,   const Eigen::Vector3d & prev_p,   const Eigen::Vector3d & p,  
           const Eigen::Vector3d & next_p ) {
           Eigen::Vector2d prev_mvmt = p.block<2,   1>(  0,   0 ) - prev_p.block<2,   1>(  0,   0 );
           Eigen::Vector2d next_mvmt = next_p.block<2,   1>(  0,   0 ) - p.block<2,   1>(  0,   0 );
           if (  i == cusp_i_ ) {
           next_mvmt = -next_mvmt;
           }
           return (  next_mvmt - prev_mvmt ).norm(   );
           };
          };
          
          TEST_F(  SmootherTest,   testingSmoothness )
          {
           // set w_curve to 0.0 so that the whole job is upon w_smooth
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   0.0 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> sharp_turn_90 =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.3,   0,   M_PI / 4},  
           {0.3,   0.1,   M_PI / 2},  
           {0.3,   0.2,   M_PI / 2},  
           {0.3,   0.3,   M_PI / 2}
           };
          
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90,   smoothed_path ) );
          
           double mvmt_smoothness_improvement =
           assessPathImprovement(  sharp_turn_90,   smoothed_path,   mvmt_smoothness_criterion_ );
           EXPECT_GT(  mvmt_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  mvmt_smoothness_improvement,   55.3,   1.0 );
          
           auto orientation_smoothness_criterion =
           [](  int,   const Eigen::Vector3d & prev_p,   const Eigen::Vector3d & p ) {
           return angles::normalize_angle(  p.z(   ) - prev_p.z(   ) );
           };
           double orientation_smoothness_improvement =
           assessPathImprovement(  sharp_turn_90,   smoothed_path,   orientation_smoothness_criterion );
           EXPECT_GT(  orientation_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  orientation_smoothness_improvement,   38.7,   1.0 );
          
           // path with a cusp
           std::vector<Eigen::Vector3d> sharp_turn_90_then_reverse =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.3,   0,   0},  
           {0.4,   0,   0},  
           {0.5,   0,   0},  
           {0.6,   0,   M_PI / 4},  
           {0.6,   -0.1,   M_PI / 2},  
           {0.6,   -0.2,   M_PI / 2},  
           {0.6,   -0.3,   M_PI / 2},  
           {0.6,   -0.4,   M_PI / 2},  
           {0.6,   -0.5,   M_PI / 2},  
           {0.6,   -0.6,   M_PI / 2}
           };
           cusp_i_ = 6;
          
           EXPECT_TRUE(  smoothPath(  sharp_turn_90_then_reverse,   smoothed_path ) );
          
           mvmt_smoothness_improvement =
           assessPathImprovement(  sharp_turn_90_then_reverse,   smoothed_path,   mvmt_smoothness_criterion_ );
           EXPECT_GT(  mvmt_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  mvmt_smoothness_improvement,   37.2,   1.0 );
          
           orientation_smoothness_improvement =
           assessPathImprovement(  
           sharp_turn_90_then_reverse,   smoothed_path,  
           orientation_smoothness_criterion );
           EXPECT_GT(  orientation_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  orientation_smoothness_improvement,   28.5,   1.0 );
          
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingAnchoringToOriginalPath )
          {
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_smooth",   30.0 ) );
           // set w_curve to 0.0,   we don't care about turning radius in this test...
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   0.0 ) );
           // first keep w_dist at 0.0 to generate an unanchored smoothed path
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> sharp_turn_90 =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.3,   0,   M_PI / 4},  
           {0.3,   0.1,   M_PI / 2},  
           {0.3,   0.2,   M_PI / 2},  
           {0.3,   0.3,   M_PI / 2}
           };
          
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90,   smoothed_path ) );
          
           // then update w_dist and compare the results
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_dist",   30.0 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> smoothed_path_anchored;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90,   smoothed_path_anchored ) );
          
           auto origin_similarity_criterion =
           [&sharp_turn_90](  int i,   const Eigen::Vector3d & p ) {
           return (  p.block<2,   1>(  0,   0 ) - sharp_turn_90[i].block<2,   1>(  0,   0 ) ).norm(   );
           };
           double origin_similarity_improvement =
           assessPathImprovement(  smoothed_path,   smoothed_path_anchored,   origin_similarity_criterion );
           EXPECT_GT(  origin_similarity_improvement,   0.0 );
           EXPECT_NEAR(  origin_similarity_improvement,   45.5,   1.0 );
          
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingMaxCurvature )
          {
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   30.0 ) );
           // set w_smooth to a small value so that the whole job is upon w_curve
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_smooth",   0.3 ) );
           // let's give the smoother more time since w_smooth is so small
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.optimizer.max_iterations",   500 ) );
           reloadParams(   );
          
           // smoother should increase radius from infeasible 0.3 to feasible 0.4
           std::vector<Eigen::Vector3d> radius_0_3_turn_90 =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.2 + 0.3 * sin(  M_PI / 12 ),   0.3 * (  1 - cos(  M_PI / 12 ) ),   0},  
           {0.2 + 0.3 * sin(  M_PI * 2 / 12 ),   0.3 * (  1 - cos(  M_PI * 2 / 12 ) ),   0},  
           {0.2 + 0.3 * sin(  M_PI * 3 / 12 ),   0.3 * (  1 - cos(  M_PI * 3 / 12 ) ),   0},  
           {0.2 + 0.3 * sin(  M_PI * 4 / 12 ),   0.3 * (  1 - cos(  M_PI * 4 / 12 ) ),   0},  
           {0.2 + 0.3 * sin(  M_PI * 5 / 12 ),   0.3 * (  1 - cos(  M_PI * 5 / 12 ) ),   0},  
           {0.5,   0.3,   M_PI / 2},  
           {0.5,   0.4,   M_PI / 2},  
           {0.5,   0.5,   M_PI / 2}
           };
          
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  radius_0_3_turn_90,   smoothed_path ) );
          
           // we don't expect result to be smoother than original as w_smooth is too low
           // but let's check for large discontinuities using a well chosen upper bound
           auto upper_bound = zigZaggedPath(  radius_0_3_turn_90,   0.01 );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path,   mvmt_smoothness_criterion_ ),   0.0 );
          
           // smoothed path points should form a circle with radius 0.4
           for (  size_t i = 1; i < smoothed_path.size(   ) - 1; i++ ) {
           auto & p = smoothed_path[i];
           double r = (  p.block<2,   1>(  0,   0 ) - Eigen::Vector2d(  0.1,   0.4 ) ).norm(   );
           EXPECT_NEAR(  r,   0.4,   0.01 );
           }
          
           // path with a cusp
           // smoother should increase radius from infeasible 0.3 to feasible 0.4
           std::vector<Eigen::Vector3d> radius_0_3_turn_90_then_reverse_turn_90 =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.2 + 0.3 * sin(  M_PI / 12 ),   0.3 * (  1 - cos(  M_PI / 12 ) ),   M_PI / 12},  
           {0.2 + 0.3 * sin(  M_PI * 2 / 12 ),   0.3 * (  1 - cos(  M_PI * 2 / 12 ) ),   M_PI *2 / 12},  
           {0.2 + 0.3 * sin(  M_PI * 3 / 12 ),   0.3 * (  1 - cos(  M_PI * 3 / 12 ) ),   M_PI *3 / 12},  
           {0.2 + 0.3 * sin(  M_PI * 4 / 12 ),   0.3 * (  1 - cos(  M_PI * 4 / 12 ) ),   M_PI *4 / 12},  
           {0.2 + 0.3 * sin(  M_PI * 5 / 12 ),   0.3 * (  1 - cos(  M_PI * 5 / 12 ) ),   M_PI *5 / 12},  
           {0.5,   0.3,   M_PI / 2},  
           {0.8 - 0.3 * sin(  M_PI * 5 / 12 ),   0.3 * (  1 - cos(  M_PI * 5 / 12 ) ),   M_PI *7 / 12},  
           {0.8 - 0.3 * sin(  M_PI * 4 / 12 ),   0.3 * (  1 - cos(  M_PI * 4 / 12 ) ),   M_PI *8 / 12},  
           {0.8 - 0.3 * sin(  M_PI * 3 / 12 ),   0.3 * (  1 - cos(  M_PI * 3 / 12 ) ),   M_PI *9 / 12},  
           {0.8 - 0.3 * sin(  M_PI * 2 / 12 ),   0.3 * (  1 - cos(  M_PI * 2 / 12 ) ),   M_PI *10 / 12},  
           {0.8 - 0.3 * sin(  M_PI / 12 ),   0.3 * (  1 - cos(  M_PI / 12 ) ),   M_PI *11 / 12},  
           {0.8,   0,   M_PI},  
           {0.9,   0,   M_PI},  
           {1.0,   0,   M_PI}
           };
          
           EXPECT_TRUE(  smoothPath(  radius_0_3_turn_90_then_reverse_turn_90,   smoothed_path ) );
          
           // we don't expect result to be smoother than original as w_smooth is too low
           // but let's check for large discontinuities using a well chosen upper bound
           cusp_i_ = 8;
           upper_bound = zigZaggedPath(  radius_0_3_turn_90_then_reverse_turn_90,   0.01 );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path,   mvmt_smoothness_criterion_ ),   0.0 );
          
           // smoothed path points should form a circle with radius 0.4
           // for both forward and reverse movements
           for (  size_t i = 1; i < smoothed_path.size(   ) - 1; i++ ) {
           auto & p = smoothed_path[i];
           double r = (  p.block<2,   1>(  0,   0 ) - Eigen::Vector2d(  i <= 8 ? 0.1 : 0.9,   0.4 ) ).norm(   );
           EXPECT_NEAR(  r,   0.4,   0.01 );
           }
          
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingObstacleAvoidance )
          {
           auto costmap = costmap_sub_->getCostmap(   );
           nav2_costmap_2d::FootprintCollisionChecker collision_checker(  costmap );
           nav2_costmap_2d::Footprint footprint;
          
           auto cost_avoidance_criterion =
           [&collision_checker,   &footprint](  int,   const Eigen::Vector3d & p ) {
           return collision_checker.footprintCostAtPose(  p[0],   p[1],   p[2],   footprint );
           };
          
           // a symmetric footprint (  diff-drive with 4 actuated wheels )
           footprint.push_back(  pointMsg(  0.4,   0.25 ) );
           footprint.push_back(  pointMsg(  -0.4,   0.25 ) );
           footprint.push_back(  pointMsg(  -0.4,   -0.25 ) );
           footprint.push_back(  pointMsg(  0.4,   -0.25 ) );
          
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_smooth",   15000.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.015 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> straight_near_obstacle =
           {{0.05,   0.05,   0},  
           {0.45,   0.05,   0},  
           {0.85,   0.05,   0},  
           {1.25,   0.05,   0},  
           {1.65,   0.05,   0},  
           {2.05,   0.05,   0},  
           {2.45,   0.05,   0},  
           {2.85,   0.05,   0},  
           {3.25,   0.05,   0},  
           {3.65,   0.05,   0},  
           {4.05,   0.05,   0}
           };
          
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  straight_near_obstacle,   smoothed_path ) );
          
           // we don't expect result to be smoother than original as original straight line was 100% smooth
           // but let's check for large discontinuities using a well chosen upper bound
           auto upper_bound = zigZaggedPath(  straight_near_obstacle,   0.01 );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path,   mvmt_smoothness_criterion_ ),   0.0 );
          
           double cost_avoidance_improvement = assessPathImprovement(  
           straight_near_obstacle,   smoothed_path,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement,   0.0 );
           EXPECT_NEAR(  cost_avoidance_improvement,   12.9,   1.0 );
          }
          
          TEST_F(  SmootherTest,   testingObstacleAvoidanceNearCusps )
          {
           auto costmap = costmap_sub_->getCostmap(   );
           nav2_costmap_2d::FootprintCollisionChecker collision_checker(  costmap );
           nav2_costmap_2d::Footprint footprint;
          
           auto cost_avoidance_criterion =
           [&collision_checker,   &footprint](  int,   const Eigen::Vector3d & p ) {
           return collision_checker.footprintCostAtPose(  p[0],   p[1],   p[2],   footprint );
           };
          
           // path with a cusp
           std::vector<Eigen::Vector3d> cusp_near_obstacle =
           {{0.05,   0.05,   0},  
           {0.15,   0.05,   0},  
           {0.25,   0.05,   0},  
           {0.35,   0.05,   0},  
           {0.45,   0.05,   0},  
           {0.55,   0.05,   0},  
           {0.65,   0.05,   0},  
           {0.75,   0.05,   0},  
           {0.85,   0.05,   0},  
           {0.95,   0.05,   0},  
           {1.05,   0.05,   0},  
           {1.15,   0.05,   0},  
           {1.25,   0.05,   0},  
           {1.25 + 0.4 * sin(  M_PI / 12 ),   0.4 * (  1 - cos(  M_PI / 12 ) ) + 0.05,   M_PI / 12},  
           {1.25 + 0.4 * sin(  M_PI * 2 / 12 ),   0.4 * (  1 - cos(  M_PI * 2 / 12 ) ) + 0.05,   M_PI *2 / 12},  
           {1.25 + 0.4 * sin(  M_PI * 3 / 12 ),   0.4 * (  1 - cos(  M_PI * 3 / 12 ) ) + 0.05,   M_PI *3 / 12},  
           {1.25 + 0.4 * sin(  M_PI * 4 / 12 ),   0.4 * (  1 - cos(  M_PI * 4 / 12 ) ) + 0.05,   M_PI *4 / 12},  
           {1.25 + 0.4 * sin(  M_PI * 5 / 12 ),   0.4 * (  1 - cos(  M_PI * 5 / 12 ) ) + 0.05,   M_PI *5 / 12},  
           {1.65,   0.45,   M_PI / 2},  
           {2.05 - 0.4 * sin(  M_PI * 5 / 12 ),   0.4 * (  1 - cos(  M_PI * 5 / 12 ) ) + 0.05,   M_PI *7 / 12},  
           {2.05 - 0.4 * sin(  M_PI * 4 / 12 ),   0.4 * (  1 - cos(  M_PI * 4 / 12 ) ) + 0.05,   M_PI *8 / 12},  
           {2.05 - 0.4 * sin(  M_PI * 3 / 12 ),   0.4 * (  1 - cos(  M_PI * 3 / 12 ) ) + 0.05,   M_PI *9 / 12},  
           {2.05 - 0.4 * sin(  M_PI * 2 / 12 ),   0.4 * (  1 - cos(  M_PI * 2 / 12 ) ) + 0.05,   M_PI *10 / 12},  
           {2.05 - 0.4 * sin(  M_PI / 12 ),   0.4 * (  1 - cos(  M_PI / 12 ) ) + 0.05,   M_PI *11 / 12},  
           {2.05,   0.05,   M_PI},  
           {2.15,   0.05,   M_PI},  
           {2.25,   0.05,   M_PI},  
           {2.35,   0.05,   M_PI},  
           {2.45,   0.05,   M_PI},  
           {2.55,   0.05,   M_PI},  
           {2.65,   0.05,   M_PI},  
           {2.75,   0.05,   M_PI},  
           {2.85,   0.05,   M_PI},  
           {2.95,   0.05,   M_PI},  
           {3.05,   0.05,   M_PI},  
           {3.15,   0.05,   M_PI},  
           {3.25,   0.05,   M_PI},  
           {3.35,   0.05,   M_PI},  
           {3.45,   0.05,   M_PI},  
           {3.55,   0.05,   M_PI},  
           {3.65,   0.05,   M_PI},  
           {3.75,   0.05,   M_PI},  
           {3.85,   0.05,   M_PI},  
           {3.95,   0.05,   M_PI},  
           {4.05,   0.05,   M_PI}
           };
           cusp_i_ = 18;
          
           // we don't expect result to be smoother than original
           // but let's check for large discontinuities using a well chosen upper bound
           auto upper_bound = zigZaggedPath(  cusp_near_obstacle,   0.01 );
          
           /////////////////////////////////////////////////////
           // testing option to pay extra attention near cusps
          
           // extra attention near cusps option is more significant with a long footprint
           footprint.clear(   );
           footprint.push_back(  pointMsg(  0.4,   0.2 ) );
           footprint.push_back(  pointMsg(  -0.4,   0.2 ) );
           footprint.push_back(  pointMsg(  -0.4,   -0.2 ) );
           footprint.push_back(  pointMsg(  0.4,   -0.2 ) );
          
           // first smooth with homogeneous w_cost to compare
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.015 ) );
           // higher w_curve significantly decreases convergence speed here
           // path feasibility can be restored by subsequent resmoothing with higher w_curve
           // TODO(  afrixs ): tune ceres optimizer to "converge" faster,  
           // see http://ceres-solver.org/nnls_solving.html
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   1.0 ) );
           // let's have more iterations so that the improvement is more significant
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.optimizer.max_iterations",   500 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path,   true,   true ) );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path,   mvmt_smoothness_criterion_ ),   0.0 );
           double cost_avoidance_improvement_simple = assessPathImprovement(  
           cusp_near_obstacle,  
           smoothed_path,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement_simple,   0.0 );
           EXPECT_NEAR(  cost_avoidance_improvement_simple,   42.6,   1.0 );
           double worst_cost_improvement_simple = assessWorstPoseImprovement(  
           cusp_near_obstacle,  
           smoothed_path,  
           cost_avoidance_criterion );
           RCLCPP_INFO(  
           rclcpp::get_logger(  "ceres_smoother" ),   "Cost avoidance improvement (  cusp,   simple ): %lf,   %lf",  
           cost_avoidance_improvement_simple,   worst_cost_improvement_simple );
           EXPECT_GE(  worst_cost_improvement_simple,   0.0 );
          
          
           // then update parameters so that robot is not so afraid of obstacles
           // during simple movement but pays extra attention during rotations near cusps
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.0052 ) );
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  "SmoothPath.w_cost_cusp_multiplier",   0.027 / 0.0052 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.cusp_zone_length",   2.5 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.optimizer.fn_tol",   1e-15 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> smoothed_path_ecc;
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path_ecc,   true,   false ) );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path_ecc,   mvmt_smoothness_criterion_ ),   0.0 );
           double cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement(  
           cusp_near_obstacle,  
           smoothed_path_ecc,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement_extra_careful_cusp,   0.0 );
           EXPECT_NEAR(  cost_avoidance_improvement_extra_careful_cusp,   44.2,   1.0 );
           double worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement(  
           cusp_near_obstacle,  
           smoothed_path_ecc,  
           cost_avoidance_criterion );
           RCLCPP_INFO(  
           rclcpp::get_logger(  "ceres_smoother" ),   "Cost avoidance improvement (  cusp,   ecc ): %lf,   %lf",  
           cost_avoidance_improvement_extra_careful_cusp,   worst_cost_improvement_extra_careful_cusp );
           EXPECT_GE(  worst_cost_improvement_extra_careful_cusp,   0.0 );
           EXPECT_GE(  worst_cost_improvement_extra_careful_cusp,   worst_cost_improvement_simple );
           EXPECT_GT(  cost_avoidance_improvement_extra_careful_cusp,   cost_avoidance_improvement_simple );
          
           // although extra careful cusp optimization avoids cost better than simple one,  
           // overall the path doesn't need to deflect so much from original,   since w_cost is smaller
           // and thus the obstacles are avoided mostly in dangerous zones around cusps
           auto origin_similarity_criterion =
           [&cusp_near_obstacle](  int i,   const Eigen::Vector3d & p ) {
           return (  p.block<2,   1>(  0,   0 ) - cusp_near_obstacle[i].block<2,   1>(  0,   0 ) ).norm(   );
           };
           double origin_similarity_improvement =
           assessPathImprovement(  smoothed_path,   smoothed_path_ecc,   origin_similarity_criterion );
           RCLCPP_INFO(  
           rclcpp::get_logger(  
           "ceres_smoother" ),   "Original similarity improvement (  cusp,   ecc vs. simple ): %lf",  
           origin_similarity_improvement );
           EXPECT_GT(  origin_similarity_improvement,   0.0 );
           EXPECT_NEAR(  origin_similarity_improvement,   0.43,   0.02 );
          
          
           /////////////////////////////////////////////////////
           // testing asymmetric footprint options
          
           // (  diff-drive with 2 actuated wheels and 2 caster wheels )
           footprint.clear(   );
           footprint.push_back(  pointMsg(  0.15,   0.2 ) );
           footprint.push_back(  pointMsg(  -0.65,   0.2 ) );
           footprint.push_back(  pointMsg(  -0.65,   -0.2 ) );
           footprint.push_back(  pointMsg(  0.15,   -0.2 ) );
          
           // reset parameters back to homogeneous and shift cost check point to the center of the footprint
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_smooth",   15000 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_curve",   1.0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.015 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.cusp_zone_length",   -1.0 ) );
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(  {-0.05,   0.0,   0.5,   -0.45,   0.0,   0.5} ) // x1,   y1,   weight1,   x2,   y2,   weight2
            ) );
           reloadParams(   );
          
           // cost improvement is different for path smoothed by original optimizer
           // since the footprint has changed
           cost_avoidance_improvement_simple = assessPathImprovement(  
           cusp_near_obstacle,   smoothed_path,  
           cost_avoidance_criterion );
           worst_cost_improvement_simple = assessWorstPoseImprovement(  
           cusp_near_obstacle,   smoothed_path,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement_simple,   0.0 );
           RCLCPP_INFO(  
           rclcpp::get_logger(  
           "ceres_smoother" ),   "Cost avoidance improvement (  cusp_shifted,   simple ): %lf,   %lf",  
           cost_avoidance_improvement_simple,   worst_cost_improvement_simple );
           EXPECT_NEAR(  cost_avoidance_improvement_simple,   40.2,   1.0 );
          
           // now smooth using the new optimizer with cost check point shifted
           std::vector<Eigen::Vector3d> smoothed_path_scc;
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path_scc ) );
           EXPECT_GT(  assessPathImprovement(  upper_bound,   smoothed_path_scc,   mvmt_smoothness_criterion_ ),   0.0 );
           double cost_avoidance_improvement_shifted_cost_check = assessPathImprovement(  
           cusp_near_obstacle,  
           smoothed_path_scc,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement_shifted_cost_check,   0.0 );
           EXPECT_NEAR(  cost_avoidance_improvement_shifted_cost_check,   42.0,   1.0 );
           double worst_cost_improvement_shifted_cost_check = assessWorstPoseImprovement(  
           cusp_near_obstacle,  
           smoothed_path_scc,  
           cost_avoidance_criterion );
           RCLCPP_INFO(  
           rclcpp::get_logger(  
           "ceres_smoother" ),   "Cost avoidance improvement (  cusp_shifted,   scc ): %lf,   %lf",  
           cost_avoidance_improvement_shifted_cost_check,   worst_cost_improvement_shifted_cost_check );
           EXPECT_GE(  worst_cost_improvement_shifted_cost_check,   0.0 );
           EXPECT_GE(  worst_cost_improvement_shifted_cost_check,   worst_cost_improvement_simple );
           EXPECT_GT(  cost_avoidance_improvement_shifted_cost_check,   cost_avoidance_improvement_simple );
          
           // same results should be achieved with unnormalized weights
           // (  testing automatic weights normalization,   i.e. using avg instead of sum )
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(  {-0.05,   0.0,   1.0,   -0.45,   0.0,   1.0} ) // x1,   y1,   weight1,   x2,   y2,   weight2
            ) );
           reloadParams(   );
           std::vector<Eigen::Vector3d> smoothed_path_scc_unnormalized;
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path_scc_unnormalized ) );
           EXPECT_EQ(  smoothed_path_scc,   smoothed_path_scc_unnormalized );
          
           ////////////////////////////////////////
           // compare also with extra careful cusp
          
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.w_cost",   0.0052 ) );
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  "SmoothPath.w_cost_cusp_multiplier",   0.027 / 0.0052 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.cusp_zone_length",   2.5 ) );
           // we need much more iterations here since it's a more complicated problem
           // TODO(  afrixs ): tune ceres optimizer to "converge" faster
           // see http://ceres-solver.org/nnls_solving.html
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.optimizer.max_iterations",   1500 ) );
           reloadParams(   );
          
           std::vector<Eigen::Vector3d> smoothed_path_scce;
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path_scce ) );
           EXPECT_GT(  
           assessPathImprovement(  upper_bound,   smoothed_path_scce,   mvmt_smoothness_criterion_ ),  
           0.0 );
           double cost_avoidance_improvement_shifted_extra = assessPathImprovement(  
           cusp_near_obstacle,  
           smoothed_path_scce,  
           cost_avoidance_criterion );
           double worst_cost_improvement_shifted_extra = assessWorstPoseImprovement(  
           cusp_near_obstacle,  
           smoothed_path_scce,  
           cost_avoidance_criterion );
           RCLCPP_INFO(  
           rclcpp::get_logger(  
           "ceres_smoother" ),   "Cost avoidance improvement (  cusp_shifted,   scce ): %lf,   %lf",  
           cost_avoidance_improvement_shifted_extra,   worst_cost_improvement_shifted_extra );
           EXPECT_NEAR(  cost_avoidance_improvement_shifted_extra,   51.0,   1.0 );
           EXPECT_GE(  worst_cost_improvement_shifted_extra,   0.0 );
          
           // resmooth extra careful cusp with same conditions (  higher max_iterations )
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(   ) ) );
           reloadParams(   );
          
           EXPECT_TRUE(  smoothPath(  cusp_near_obstacle,   smoothed_path_ecc ) );
           cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement(  
           cusp_near_obstacle,  
           smoothed_path_ecc,  
           cost_avoidance_criterion );
           worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement(  
           cusp_near_obstacle,  
           smoothed_path_ecc,  
           cost_avoidance_criterion );
           EXPECT_GT(  cost_avoidance_improvement_extra_careful_cusp,   0.0 );
           RCLCPP_INFO(  
           rclcpp::get_logger(  
           "ceres_smoother" ),   "Cost avoidance improvement (  cusp_shifted,   ecc ): %lf,   %lf",  
           cost_avoidance_improvement_extra_careful_cusp,   worst_cost_improvement_extra_careful_cusp );
           EXPECT_NEAR(  cost_avoidance_improvement_extra_careful_cusp,   48.5,   1.0 );
           EXPECT_GT(  
           cost_avoidance_improvement_shifted_extra,  
           cost_avoidance_improvement_extra_careful_cusp );
           // worst cost improvement is a bit lower but only by 5% so it's not a big deal
           EXPECT_GE(  worst_cost_improvement_shifted_extra,   worst_cost_improvement_extra_careful_cusp - 6.0 );
          
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingDownsamplingUpsampling )
          {
           // path with a cusp
           std::vector<Eigen::Vector3d> sharp_turn_90_then_reverse =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.3,   0,   0},  
           {0.4,   0,   0},  
           {0.5,   0,   0},  
           {0.6,   0,   M_PI / 4},  
           {0.6,   -0.1,   M_PI / 2},  
           {0.6,   -0.2,   M_PI / 2},  
           {0.6,   -0.3,   M_PI / 2},  
           {0.6,   -0.4,   M_PI / 2},  
           {0.6,   -0.5,   M_PI / 2},  
           {0.6,   -0.6,   M_PI / 2}
           };
           cusp_i_ = 6;
          
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_downsampling_factor",   2 ) );
           // downsample only
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_upsampling_factor",   0 ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.reversing_enabled",   false ) );
           reloadParams(   );
           std::vector<Eigen::Vector3d> smoothed_path_downsampled;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90_then_reverse,   smoothed_path_downsampled ) );
           // first two,   last two and every 2nd pose between them
           EXPECT_EQ(  smoothed_path_downsampled.size(   ),   8u );
          
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.reversing_enabled",   true ) );
           reloadParams(   );
           EXPECT_TRUE(  smoothPath(  sharp_turn_90_then_reverse,   smoothed_path_downsampled ) );
           // same but downsampling is reset on cusp
           EXPECT_EQ(  smoothed_path_downsampled.size(   ),   9u );
          
           // upsample to original size
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_upsampling_factor",   1 ) );
           reloadParams(   );
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90_then_reverse,   smoothed_path ) );
           EXPECT_EQ(  smoothed_path.size(   ),   sharp_turn_90_then_reverse.size(   ) );
          
           cusp_i_ = 4; // for downsampled path
           int cusp_i_out = 6; // for upsampled path
           QualityCriterion3 mvmt_smoothness_criterion_out =
           [&cusp_i_out](  int i,   const Eigen::Vector3d & prev_p,   const Eigen::Vector3d & p,  
           const Eigen::Vector3d & next_p ) {
           Eigen::Vector2d prev_mvmt = p.block<2,   1>(  0,   0 ) - prev_p.block<2,   1>(  0,   0 );
           Eigen::Vector2d next_mvmt = next_p.block<2,   1>(  0,   0 ) - p.block<2,   1>(  0,   0 );
           if (  i == cusp_i_out ) {
           next_mvmt = -next_mvmt;
           }
           return (  next_mvmt - prev_mvmt ).norm(   );
           };
          
           double smoothness_improvement = assessPathImprovement(  
           smoothed_path_downsampled,   smoothed_path,  
           mvmt_smoothness_criterion_,  
           &mvmt_smoothness_criterion_out );
           // more poses -> smoother path
           EXPECT_GT(  smoothness_improvement,   0.0 );
           EXPECT_NEAR(  smoothness_improvement,   65.7,   1.0 );
          
           // upsample above original size
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.path_upsampling_factor",   2 ) );
           reloadParams(   );
           EXPECT_TRUE(  smoothPath(  sharp_turn_90_then_reverse,   smoothed_path ) );
           // every pose except last produces 2 poses
           EXPECT_EQ(  smoothed_path.size(   ),   sharp_turn_90_then_reverse.size(   ) * 2 - 1 );
           cusp_i_out = 12; // for upsampled path
           smoothness_improvement = assessPathImprovement(  
           smoothed_path_downsampled,   smoothed_path,  
           mvmt_smoothness_criterion_,  
           &mvmt_smoothness_criterion_out );
           // even more poses -> even smoother path
           EXPECT_GT(  smoothness_improvement,   0.0 );
           EXPECT_NEAR(  smoothness_improvement,   83.7,   1.0 );
          }
          
          TEST_F(  SmootherTest,   testingStartGoalOrientations )
          {
           std::vector<Eigen::Vector3d> sharp_turn_90 =
           {{0,   0,   0},  
           {0.1,   0,   0},  
           {0.2,   0,   0},  
           {0.3,   0,   M_PI / 4},  
           {0.3,   0.1,   M_PI / 2},  
           {0.3,   0.2,   M_PI / 2},  
           {0.3,   0.3,   M_PI / 2}
           };
          
           // Keep start and goal orientations (  by default )
           std::vector<Eigen::Vector3d> smoothed_path;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90,   smoothed_path ) );
          
           double mvmt_smoothness_improvement =
           assessPathImprovement(  sharp_turn_90,   smoothed_path,   mvmt_smoothness_criterion_ );
           EXPECT_GT(  mvmt_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  mvmt_smoothness_improvement,   53.3,   1.0 );
           // no change in orientations
           EXPECT_NEAR(  smoothed_path.front(   )[2],   0,   0.001 );
           EXPECT_NEAR(  smoothed_path.back(   )[2],   M_PI / 2,   0.001 );
          
           // Overwrite start/goal orientations
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.keep_start_orientation",   false ) );
           node_lifecycle_->set_parameter(  rclcpp::Parameter(  "SmoothPath.keep_goal_orientation",   false ) );
           reloadParams(   );
          
           sharp_turn_90[0][2] = M_PI; // forward/reverse of start pose should not matter
           std::vector<Eigen::Vector3d> smoothed_path_sg_overwritten;
           EXPECT_TRUE(  smoothPath(  sharp_turn_90,   smoothed_path_sg_overwritten ) );
          
           mvmt_smoothness_improvement =
           assessPathImprovement(  smoothed_path,   smoothed_path_sg_overwritten,   mvmt_smoothness_criterion_ );
           EXPECT_GT(  mvmt_smoothness_improvement,   0.0 );
           EXPECT_NEAR(  mvmt_smoothness_improvement,   98.3,   1.0 );
           // orientations adjusted to follow the path
           EXPECT_NEAR(  smoothed_path_sg_overwritten.front(   )[2],   M_PI / 4,   0.1 );
           EXPECT_NEAR(  smoothed_path_sg_overwritten.back(   )[2],   M_PI / 4,   0.1 );
          
           // test short paths
           std::vector<Eigen::Vector3d> short_screwed_path =
           {{0,   0,   M_PI * 0.25},  
           {0.1,   0,   -M_PI * 0.25}
           };
          
           std::vector<Eigen::Vector3d> adjusted_path;
           EXPECT_TRUE(  smoothPath(  short_screwed_path,   adjusted_path ) );
           EXPECT_NEAR(  adjusted_path.front(   )[2],   0,   0.001 );
           EXPECT_NEAR(  adjusted_path.back(   )[2],   0,   0.001 );
          
           short_screwed_path[0][2] = M_PI * 0.75; // start is stronger than goal
           EXPECT_TRUE(  smoothPath(  short_screwed_path,   adjusted_path ) );
           EXPECT_NEAR(  adjusted_path.front(   )[2],   M_PI,   0.001 );
           EXPECT_NEAR(  adjusted_path.back(   )[2],   M_PI,   0.001 );
          
           std::vector<Eigen::Vector3d> one_pose_path = {{0,   0,   M_PI * 0.75}};
           EXPECT_TRUE(  smoothPath(  one_pose_path,   adjusted_path ) );
           EXPECT_NEAR(  adjusted_path.front(   )[2],   M_PI * 0.75,   0.001 );
          }
          
          TEST_F(  SmootherTest,   testingCostCheckPointsParamValidity )
          {
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(   ) ) );
           reloadParams(   );
          
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(  {0,   0,   0,   0,   0,   0,   0,   0,   0} ) ) ); // multiple of 3 is ok
           reloadParams(   );
          
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.cost_check_points",  
           std::vector<double>(  {0,   0} ) ) );
           EXPECT_THROW(  reloadParams(   ),   std::runtime_error );
          }
          
          TEST_F(  SmootherTest,   testingLinearSolverTypeParamValidity )
          {
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.optimizer.linear_solver_type",  
           "SPARSE_NORMAL_CHOLESKY" ) );
           reloadParams(   );
          
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.optimizer.linear_solver_type",  
           "DENSE_QR" ) );
           reloadParams(   );
          
           node_lifecycle_->set_parameter(  
           rclcpp::Parameter(  
           "SmoothPath.optimizer.linear_solver_type",  
           "INVALID_SOLVER" ) );
           EXPECT_THROW(  reloadParams(   ),   std::runtime_error );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_constrained_smoother/test/test_smoother_cost_function.cpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <chrono>
          #include <iostream>
          #include <future>
          #include <thread>
          #include <algorithm>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_constrained_smoother/smoother_cost_function.hpp"
          
      28  class TestableSmootherCostFunction : nav2_constrained_smoother::SmootherCostFunction
          {
          public:
      31   TestableSmootherCostFunction(  
           const Eigen::Vector2d & original_pos,  
           double next_to_last_length_ratio,  
           bool reversing,  
           const nav2_costmap_2d::Costmap2D * costmap,  
           const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,  
           const nav2_constrained_smoother::SmootherParams & params,  
           double costmap_weight )
           : SmootherCostFunction(  
           original_pos,   next_to_last_length_ratio,   reversing,  
           costmap,   costmap_interpolator,  
           params,   costmap_weight )
           {
           }
          
      46   inline double getCurvatureResidual(  
           const double & weight,  
      48   const Eigen::Vector2d & pt,  
      49   const Eigen::Vector2d & pt_next,  
      50   const Eigen::Vector2d & pt_prev ) const
           {
           double r = 0.0;
           addCurvatureResidual<double>(  weight,   pt,   pt_next,   pt_prev,   r );
           return r;
           }
          };
          
      58  class Test : public ::testing::Test
          {
          protected:
      61   void SetUp(   )
           {
           }
          };
          
      66  TEST_F(  Test,   testingCurvatureResidual )
          {
           nav2_costmap_2d::Costmap2D costmap;
           TestableSmootherCostFunction fn(  
           Eigen::Vector2d(  1.0,   0.0 ),   1.0,   false,  
           &costmap,   std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(   ),  
           nav2_constrained_smoother::SmootherParams(   ),   0.0
            );
          
           // test for edge values
           Eigen::Vector2d pt(  1.0,   0.0 );
           Eigen::Vector2d pt_other(  0.0,   0.0 );
           EXPECT_EQ(  fn.getCurvatureResidual(  0.0,   pt,   pt_other,   pt_other ),   0.0 );
          
           nav2_constrained_smoother::SmootherParams params_no_min_turning_radius;
           params_no_min_turning_radius.max_curvature = 1.0f / 0.0;
           TestableSmootherCostFunction fn_no_min_turning_radius(  
           Eigen::Vector2d(  1.0,   0.0 ),   1.0,   false,  
           &costmap,   std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(   ),  
           params_no_min_turning_radius,   0.0
            );
           EXPECT_EQ(  fn_no_min_turning_radius.getCurvatureResidual(  1.0,   pt,   pt_other,   pt_other ),   0.0 );
          }
          
      90  TEST_F(  Test,   testingUtils )
          {
           Eigen::Vector2d pt(  1.0,   0.0 );
           Eigen::Vector2d pt_prev(  0.0,   0.0 );
           Eigen::Vector2d pt_next(  0.0,   0.0 );
          
           // test for intermediate values
           auto center = nav2_constrained_smoother::arcCenter(  pt_prev,   pt,   pt_next,   false );
           // although in this situation the center would be at (  0.5,   0.0 ),  
           // cases where pt_prev == pt_next are very rare and thus unhandled
           // during the smoothing points will be separated (  and thus made valid ) by smoothness cost anyways
           EXPECT_EQ(  center[0],   std::numeric_limits<double>::infinity(   ) );
           EXPECT_EQ(  center[1],   std::numeric_limits<double>::infinity(   ) );
          
           auto tangent =
           nav2_constrained_smoother::tangentDir(  pt_prev,   pt,   pt_next,   false ).normalized(   );
           EXPECT_NEAR(  tangent[0],   0,   1e-10 );
           EXPECT_NEAR(  std::abs(  tangent[1] ),   1,   1e-10 );
          
           // no rotation when mid point is a cusp
           tangent = nav2_constrained_smoother::tangentDir(  pt_prev,   pt,   pt_next,   true ).normalized(   );
           EXPECT_NEAR(  std::abs(  tangent[0] ),   1,   1e-10 );
           EXPECT_NEAR(  tangent[1],   0,   1e-10 );
          
           pt_prev[0] = -1.0;
           // rotation is mathematically invalid,   picking direction of a shorter segment
           tangent = nav2_constrained_smoother::tangentDir(  pt_prev,   pt,   pt_next,   true ).normalized(   );
           EXPECT_NEAR(  std::abs(  tangent[0] ),   1,   1e-10 );
           EXPECT_NEAR(  tangent[1],   0,   1e-10 );
          
           pt_prev[0] = 0.0;
           pt_next[0] = -1.0;
           // rotation is mathematically invalid,   picking direction of a shorter segment
           tangent = nav2_constrained_smoother::tangentDir(  pt_prev,   pt,   pt_next,   true ).normalized(   );
           EXPECT_NEAR(  std::abs(  tangent[0] ),   1,   1e-10 );
           EXPECT_NEAR(  tangent[1],   0,   1e-10 );
          }
          
     128  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_controller/include/nav2_controller/controller_server.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
          #define NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
          
          #include <memory>
          #include <string>
          #include <thread>
          #include <unordered_map>
          #include <vector>
          #include <mutex>
          
          #include "nav2_core/controller.hpp"
          #include "nav2_core/progress_checker.hpp"
          #include "nav2_core/goal_checker.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "nav2_msgs/action/follow_path.hpp"
          #include "nav2_msgs/msg/speed_limit.hpp"
          #include "nav_2d_utils/odom_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          namespace nav2_controller
          {
          
      42  class ProgressChecker;
          /**
           * @class nav2_controller::ControllerServer
           * @brief This class hosts variety of plugins of different algorithms to
           * complete control tasks from the exposed FollowPath action server.
           */
      48  class ControllerServer : public nav2_util::LifecycleNode
          {
          public:
           using ControllerMap = std::unordered_map<std::string,   nav2_core::Controller::Ptr>;
           using GoalCheckerMap = std::unordered_map<std::string,   nav2_core::GoalChecker::Ptr>;
          
           /**
           * @brief Constructor for nav2_controller::ControllerServer
           * @param options Additional options to control creation of the node.
           */
           explicit ControllerServer(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief Destructor for nav2_controller::ControllerServer
           */
           ~ControllerServer(   );
          
          protected:
           /**
           * @brief Configures controller parameters and member variables
           *
           * Configures controller plugin and costmap; Initialize odom subscriber,  
           * velocity publisher and follow path action server.
           * @param state LifeCycle Node's state
           * @return Success or Failure
           * @throw pluginlib::PluginlibException When failed to initialize controller
           * plugin
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Activates member variables
           *
           * Activates controller,   costmap,   velocity publisher and follow path action
           * server
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Deactivates member variables
           *
           * Deactivates follow path action server,   controller,   costmap and velocity
           * publisher. Before calling deactivate state,   velocity is being set to zero.
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Calls clean up states and resets member variables.
           *
           * Controller and costmap clean up state is called,   and resets rest of the
           * variables
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in Shutdown state
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           using Action = nav2_msgs::action::FollowPath;
           using ActionServer = nav2_util::SimpleActionServer<Action>;
          
           // Our action server implements the FollowPath action
           std::unique_ptr<ActionServer> action_server_;
          
           /**
           * @brief FollowPath action server callback. Handles action server updates and
           * spins server until goal is reached
           *
           * Provides global path to controller received from action client. Twist
           * velocities for the robot are calculated and published using controller at
           * the specified rate till the goal is reached.
           * @throw nav2_core::PlannerException
           */
           void computeControl(   );
          
           /**
           * @brief Find the valid controller ID name for the given request
           *
           * @param c_name The requested controller name
           * @param name Reference to the name to use for control if any valid available
           * @return bool Whether it found a valid controller to use
           */
           bool findControllerId(  const std::string & c_name,   std::string & name );
          
           /**
           * @brief Find the valid goal checker ID name for the specified parameter
           *
           * @param c_name The goal checker name
           * @param name Reference to the name to use for goal checking if any valid available
           * @return bool Whether it found a valid goal checker to use
           */
           bool findGoalCheckerId(  const std::string & c_name,   std::string & name );
          
           /**
           * @brief Assigns path to controller
           * @param path Path received from action server
           */
           void setPlannerPath(  const nav_msgs::msg::Path & path );
           /**
           * @brief Calculates velocity and publishes to "cmd_vel" topic
           */
           void computeAndPublishVelocity(   );
           /**
           * @brief Calls setPlannerPath method with an updated path received from
           * action server
           */
           void updateGlobalPath(   );
           /**
           * @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
           * @param velocity Twist velocity to be published
           */
           void publishVelocity(  const geometry_msgs::msg::TwistStamped & velocity );
           /**
           * @brief Calls velocity publisher to publish zero velocity
           */
           void publishZeroVelocity(   );
           /**
           * @brief Checks if goal is reached
           * @return true or false
           */
           bool isGoalReached(   );
           /**
           * @brief Obtain current pose of the robot
           * @param pose To store current pose of the robot
           * @return true if able to obtain current pose of the robot,   else false
           */
           bool getRobotPose(  geometry_msgs::msg::PoseStamped & pose );
          
           /**
           * @brief get the thresholded velocity
           * @param velocity The current velocity from odometry
           * @param threshold The minimum velocity to return non-zero
           * @return double velocity value
           */
           double getThresholdedVelocity(  double velocity,   double threshold )
           {
           return (  std::abs(  velocity ) > threshold ) ? velocity : 0.0;
           }
          
           /**
           * @brief get the thresholded Twist
           * @param Twist The current Twist from odometry
           * @return Twist Twist after thresholds applied
           */
     196   nav_2d_msgs::msg::Twist2D getThresholdedTwist(  const nav_2d_msgs::msg::Twist2D & twist )
           {
           nav_2d_msgs::msg::Twist2D twist_thresh;
           twist_thresh.x = getThresholdedVelocity(  twist.x,   min_x_velocity_threshold_ );
           twist_thresh.y = getThresholdedVelocity(  twist.y,   min_y_velocity_threshold_ );
           twist_thresh.theta = getThresholdedVelocity(  twist.theta,   min_theta_velocity_threshold_ );
           return twist_thresh;
           }
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
     210   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           std::mutex dynamic_params_lock_;
          
           // The controller needs a costmap node
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
           std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
          
           // Publishers and subscribers
           std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
           rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
          
           // Progress Checker Plugin
           pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
           nav2_core::ProgressChecker::Ptr progress_checker_;
           std::string default_progress_checker_id_;
           std::string default_progress_checker_type_;
           std::string progress_checker_id_;
           std::string progress_checker_type_;
          
           // Goal Checker Plugin
           pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
           GoalCheckerMap goal_checkers_;
           std::vector<std::string> default_goal_checker_ids_;
           std::vector<std::string> default_goal_checker_types_;
           std::vector<std::string> goal_checker_ids_;
           std::vector<std::string> goal_checker_types_;
           std::string goal_checker_ids_concat_,   current_goal_checker_;
          
           // Controller Plugins
           pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
           ControllerMap controllers_;
           std::vector<std::string> default_ids_;
           std::vector<std::string> default_types_;
           std::vector<std::string> controller_ids_;
           std::vector<std::string> controller_types_;
           std::string controller_ids_concat_,   current_controller_;
          
           double controller_frequency_;
           double min_x_velocity_threshold_;
           double min_y_velocity_threshold_;
           double min_theta_velocity_threshold_;
          
           double failure_tolerance_;
          
           // Whether we've published the single controller warning yet
           geometry_msgs::msg::PoseStamped end_pose_;
          
           // Last time the controller generated a valid command
           rclcpp::Time last_valid_cmd_time_;
          
           // Current path container
           nav_msgs::msg::Path current_path_;
          
          private:
           /**
           * @brief Callback for speed limiting messages
           * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
           */
     272   void speedLimitCallback(  const nav2_msgs::msg::SpeedLimit::SharedPtr msg );
          };
          
          } // namespace nav2_controller
          
          #endif // NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_

navigation2/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
          #define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_core/goal_checker.hpp"
          #include "rcl_interfaces/msg/set_parameters_result.hpp"
          
          namespace nav2_controller
          {
          
          /**
           * @class SimpleGoalChecker
           * @brief Goal Checker plugin that only checks the position difference
           *
           * This class can be stateful if the stateful parameter is set to true (  which it is by default ).
           * This means that the goal checker will not check if the xy position matches again once it is found to be true.
           */
      57  class SimpleGoalChecker : public nav2_core::GoalChecker
          {
          public:
      60   SimpleGoalChecker(   );
           // Standard GoalChecker Interface
           void initialize(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & plugin_name,  
           const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
           void reset(   ) override;
           bool isGoalReached(  
           const geometry_msgs::msg::Pose & query_pose,   const geometry_msgs::msg::Pose & goal_pose,  
           const geometry_msgs::msg::Twist & velocity ) override;
           bool getTolerances(  
           geometry_msgs::msg::Pose & pose_tolerance,  
           geometry_msgs::msg::Twist & vel_tolerance ) override;
          
          protected:
           double xy_goal_tolerance_,   yaw_goal_tolerance_;
           bool stateful_,   check_xy_;
           // Cached squared xy_goal_tolerance_
           double xy_goal_tolerance_sq_;
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           std::string plugin_name_;
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          
          } // namespace nav2_controller
          
          #endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_

navigation2/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
          #define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
          
          #include <string>
          #include <vector>
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_core/progress_checker.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          
          namespace nav2_controller
          {
          /**
          * @class SimpleProgressChecker
          * @brief This plugin is used to check the position of the robot to make sure
          * that it is actually progressing towards a goal.
          */
          
      34  class SimpleProgressChecker : public nav2_core::ProgressChecker
          {
          public:
           void initialize(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & plugin_name ) override;
           bool check(  geometry_msgs::msg::PoseStamped & current_pose ) override;
           void reset(   ) override;
          
          protected:
           /**
           * @brief Calculates robots movement from baseline pose
           * @param pose Current pose of the robot
           * @return true,   if movement is greater than radius_,   or false
           */
           bool is_robot_moved_enough(  const geometry_msgs::msg::Pose2D & pose );
           /**
           * @brief Resets baseline pose with the current pose of the robot
           * @param pose Current pose of the robot
           */
           void reset_baseline_pose(  const geometry_msgs::msg::Pose2D & pose );
          
           rclcpp::Clock::SharedPtr clock_;
          
           double radius_;
           rclcpp::Duration time_allowance_{0,   0};
          
           geometry_msgs::msg::Pose2D baseline_pose_;
           rclcpp::Time baseline_time_;
          
           bool baseline_pose_set_{false};
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           std::string plugin_name_;
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
      74   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          } // namespace nav2_controller
          
          #endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_

navigation2/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
          #define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_controller/plugins/simple_goal_checker.hpp"
          
          namespace nav2_controller
          {
          
          /**
           * @class StoppedGoalChecker
           * @brief Goal Checker plugin that checks the position difference and velocity
           */
      53  class StoppedGoalChecker : public SimpleGoalChecker
          {
          public:
      56   StoppedGoalChecker(   );
           // Standard GoalChecker Interface
           void initialize(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & plugin_name,  
           const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
           bool isGoalReached(  
           const geometry_msgs::msg::Pose & query_pose,   const geometry_msgs::msg::Pose & goal_pose,  
           const geometry_msgs::msg::Twist & velocity ) override;
           bool getTolerances(  
           geometry_msgs::msg::Pose & pose_tolerance,  
           geometry_msgs::msg::Twist & vel_tolerance ) override;
          
          protected:
           double rot_stopped_velocity_,   trans_stopped_velocity_;
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           std::string plugin_name_;
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          
          } // namespace nav2_controller
          
          #endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_

navigation2/nav2_controller/plugins/simple_goal_checker.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          #include <limits>
          #include <vector>
          #include "nav2_controller/plugins/simple_goal_checker.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "angles/angles.h"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_controller
          {
          
      55  SimpleGoalChecker::SimpleGoalChecker(   )
          : xy_goal_tolerance_(  0.25 ),  
           yaw_goal_tolerance_(  0.25 ),  
           stateful_(  true ),  
           check_xy_(  true ),  
           xy_goal_tolerance_sq_(  0.0625 )
          {
          }
          
      64  void SimpleGoalChecker::initialize(  
      65   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      66   const std::string & plugin_name,  
      67   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/ )
          {
           plugin_name_ = plugin_name;
           auto node = parent.lock(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".xy_goal_tolerance",   rclcpp::ParameterValue(  0.25 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".yaw_goal_tolerance",   rclcpp::ParameterValue(  0.25 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".stateful",   rclcpp::ParameterValue(  true ) );
          
           node->get_parameter(  plugin_name + ".xy_goal_tolerance",   xy_goal_tolerance_ );
           node->get_parameter(  plugin_name + ".yaw_goal_tolerance",   yaw_goal_tolerance_ );
           node->get_parameter(  plugin_name + ".stateful",   stateful_ );
          
           xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &SimpleGoalChecker::dynamicParametersCallback,   this,   _1 ) );
          }
          
      93  void SimpleGoalChecker::reset(   )
          {
           check_xy_ = true;
          }
          
      98  bool SimpleGoalChecker::isGoalReached(  
      99   const geometry_msgs::msg::Pose & query_pose,   const geometry_msgs::msg::Pose & goal_pose,  
     100   const geometry_msgs::msg::Twist & )
          {
           if (  check_xy_ ) {
           double dx = query_pose.position.x - goal_pose.position.x,  
           dy = query_pose.position.y - goal_pose.position.y;
           if (  dx * dx + dy * dy > xy_goal_tolerance_sq_ ) {
           return false;
           }
           // We are within the window
           // If we are stateful,   change the state.
           if (  stateful_ ) {
           check_xy_ = false;
           }
           }
           double dyaw = angles::shortest_angular_distance(  
           tf2::getYaw(  query_pose.orientation ),  
           tf2::getYaw(  goal_pose.orientation ) );
           return fabs(  dyaw ) < yaw_goal_tolerance_;
          }
          
     120  bool SimpleGoalChecker::getTolerances(  
     121   geometry_msgs::msg::Pose & pose_tolerance,  
     122   geometry_msgs::msg::Twist & vel_tolerance )
          {
           double invalid_field = std::numeric_limits<double>::lowest(   );
          
           pose_tolerance.position.x = xy_goal_tolerance_;
           pose_tolerance.position.y = xy_goal_tolerance_;
           pose_tolerance.position.z = invalid_field;
           pose_tolerance.orientation =
           nav2_util::geometry_utils::orientationAroundZAxis(  yaw_goal_tolerance_ );
          
           vel_tolerance.linear.x = invalid_field;
           vel_tolerance.linear.y = invalid_field;
           vel_tolerance.linear.z = invalid_field;
          
           vel_tolerance.angular.x = invalid_field;
           vel_tolerance.angular.y = invalid_field;
           vel_tolerance.angular.z = invalid_field;
          
           return true;
          }
          
          rcl_interfaces::msg::SetParametersResult
     144  SimpleGoalChecker::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           for (  auto & parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".xy_goal_tolerance" ) {
           xy_goal_tolerance_ = parameter.as_double(   );
           xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
           } else if (  name == plugin_name_ + ".yaw_goal_tolerance" ) {
           yaw_goal_tolerance_ = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == plugin_name_ + ".stateful" ) {
           stateful_ = parameter.as_bool(   );
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_controller
          
     170  PLUGINLIB_EXPORT_CLASS(  nav2_controller::SimpleGoalChecker,   nav2_core::GoalChecker )

navigation2/nav2_controller/plugins/simple_progress_checker.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_controller/plugins/simple_progress_checker.hpp"
          #include <cmath>
          #include <string>
          #include <memory>
          #include <vector>
          #include "nav2_core/exceptions.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_controller
          {
      32  static double pose_distance(  const geometry_msgs::msg::Pose2D &,   const geometry_msgs::msg::Pose2D & );
          
      34  void SimpleProgressChecker::initialize(  
      35   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      36   const std::string & plugin_name )
          {
           plugin_name_ = plugin_name;
           auto node = parent.lock(   );
          
           clock_ = node->get_clock(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".required_movement_radius",   rclcpp::ParameterValue(  0.5 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".movement_time_allowance",   rclcpp::ParameterValue(  10.0 ) );
           // Scale is set to 0 by default,   so if it was not set otherwise,   set to 0
           node->get_parameter_or(  plugin_name + ".required_movement_radius",   radius_,   0.5 );
           double time_allowance_param = 0.0;
           node->get_parameter_or(  plugin_name + ".movement_time_allowance",   time_allowance_param,   10.0 );
           time_allowance_ = rclcpp::Duration::from_seconds(  time_allowance_param );
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &SimpleProgressChecker::dynamicParametersCallback,   this,   _1 ) );
          }
          
      58  bool SimpleProgressChecker::check(  geometry_msgs::msg::PoseStamped & current_pose )
          {
           // relies on short circuit evaluation to not call is_robot_moved_enough if
           // baseline_pose is not set.
           geometry_msgs::msg::Pose2D current_pose2d;
           current_pose2d = nav_2d_utils::poseToPose2D(  current_pose.pose );
          
           if (  (  !baseline_pose_set_ ) || (  is_robot_moved_enough(  current_pose2d ) ) ) {
           reset_baseline_pose(  current_pose2d );
           return true;
           }
           return !(  (  clock_->now(   ) - baseline_time_ ) > time_allowance_ );
          }
          
      72  void SimpleProgressChecker::reset(   )
          {
           baseline_pose_set_ = false;
          }
          
      77  void SimpleProgressChecker::reset_baseline_pose(  const geometry_msgs::msg::Pose2D & pose )
          {
           baseline_pose_ = pose;
           baseline_time_ = clock_->now(   );
           baseline_pose_set_ = true;
          }
          
      84  bool SimpleProgressChecker::is_robot_moved_enough(  const geometry_msgs::msg::Pose2D & pose )
          {
           return pose_distance(  pose,   baseline_pose_ ) > radius_;
          }
          
      89  static double pose_distance(  
           const geometry_msgs::msg::Pose2D & pose1,  
           const geometry_msgs::msg::Pose2D & pose2 )
          {
           double dx = pose1.x - pose2.x;
           double dy = pose1.y - pose2.y;
          
           return std::hypot(  dx,   dy );
          }
          
          rcl_interfaces::msg::SetParametersResult
     100  SimpleProgressChecker::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".required_movement_radius" ) {
           radius_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".movement_time_allowance" ) {
           time_allowance_ = rclcpp::Duration::from_seconds(  parameter.as_double(   ) );
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_controller
          
     121  PLUGINLIB_EXPORT_CLASS(  nav2_controller::SimpleProgressChecker,   nav2_core::ProgressChecker )

navigation2/nav2_controller/plugins/stopped_goal_checker.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <cmath>
          #include <string>
          #include <memory>
          #include <limits>
          #include <vector>
          #include "nav2_controller/plugins/stopped_goal_checker.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_util/node_utils.hpp"
          
          using std::hypot;
          using std::fabs;
          
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_controller
          {
          
      53  StoppedGoalChecker::StoppedGoalChecker(   )
          : SimpleGoalChecker(   ),   rot_stopped_velocity_(  0.25 ),   trans_stopped_velocity_(  0.25 )
          {
          }
          
      58  void StoppedGoalChecker::initialize(  
      59   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      60   const std::string & plugin_name,  
      61   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           plugin_name_ = plugin_name;
           SimpleGoalChecker::initialize(  parent,   plugin_name,   costmap_ros );
          
           auto node = parent.lock(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".rot_stopped_velocity",   rclcpp::ParameterValue(  0.25 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".trans_stopped_velocity",   rclcpp::ParameterValue(  0.25 ) );
          
           node->get_parameter(  plugin_name + ".rot_stopped_velocity",   rot_stopped_velocity_ );
           node->get_parameter(  plugin_name + ".trans_stopped_velocity",   trans_stopped_velocity_ );
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &StoppedGoalChecker::dynamicParametersCallback,   this,   _1 ) );
          }
          
      83  bool StoppedGoalChecker::isGoalReached(  
      84   const geometry_msgs::msg::Pose & query_pose,   const geometry_msgs::msg::Pose & goal_pose,  
      85   const geometry_msgs::msg::Twist & velocity )
          {
           bool ret = SimpleGoalChecker::isGoalReached(  query_pose,   goal_pose,   velocity );
           if (  !ret ) {
           return ret;
           }
          
           return fabs(  velocity.angular.z ) <= rot_stopped_velocity_ &&
           hypot(  velocity.linear.x,   velocity.linear.y ) <= trans_stopped_velocity_;
          }
          
      96  bool StoppedGoalChecker::getTolerances(  
      97   geometry_msgs::msg::Pose & pose_tolerance,  
      98   geometry_msgs::msg::Twist & vel_tolerance )
          {
           double invalid_field = std::numeric_limits<double>::lowest(   );
          
           // populate the poses
           bool rtn = SimpleGoalChecker::getTolerances(  pose_tolerance,   vel_tolerance );
          
           // override the velocities
           vel_tolerance.linear.x = trans_stopped_velocity_;
           vel_tolerance.linear.y = trans_stopped_velocity_;
           vel_tolerance.linear.z = invalid_field;
          
           vel_tolerance.angular.x = invalid_field;
           vel_tolerance.angular.y = invalid_field;
           vel_tolerance.angular.z = rot_stopped_velocity_;
          
           return true && rtn;
          }
          
          rcl_interfaces::msg::SetParametersResult
     118  StoppedGoalChecker::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".rot_stopped_velocity" ) {
           rot_stopped_velocity_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".trans_stopped_velocity" ) {
           trans_stopped_velocity_ = parameter.as_double(   );
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_controller
          
     139  PLUGINLIB_EXPORT_CLASS(  nav2_controller::StoppedGoalChecker,   nav2_core::GoalChecker )

navigation2/nav2_controller/plugins/test/goal_checker.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "nav2_controller/plugins/simple_goal_checker.hpp"
          #include "nav2_controller/plugins/stopped_goal_checker.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          using nav2_controller::SimpleGoalChecker;
          using nav2_controller::StoppedGoalChecker;
          
      47  void checkMacro(  
           nav2_core::GoalChecker & gc,  
           double x0,   double y0,   double theta0,  
           double x1,   double y1,   double theta1,  
           double xv,   double yv,   double thetav,  
           bool expected_result )
          {
           gc.reset(   );
           geometry_msgs::msg::Pose2D pose0,   pose1;
           pose0.x = x0;
           pose0.y = y0;
           pose0.theta = theta0;
           pose1.x = x1;
           pose1.y = y1;
           pose1.theta = theta1;
           nav_2d_msgs::msg::Twist2D v;
           v.x = xv;
           v.y = yv;
           v.theta = thetav;
           if (  expected_result ) {
           EXPECT_TRUE(  
           gc.isGoalReached(  
           nav_2d_utils::pose2DToPose(  pose0 ),  
           nav_2d_utils::pose2DToPose(  pose1 ),   nav_2d_utils::twist2Dto3D(  v ) ) );
           } else {
           EXPECT_FALSE(  
           gc.isGoalReached(  
           nav_2d_utils::pose2DToPose(  pose0 ),  
           nav_2d_utils::pose2DToPose(  pose1 ),   nav_2d_utils::twist2Dto3D(  v ) ) );
           }
          }
          
      79  void sameResult(  
           nav2_core::GoalChecker & gc0,   nav2_core::GoalChecker & gc1,  
           double x0,   double y0,   double theta0,  
           double x1,   double y1,   double theta1,  
           double xv,   double yv,   double thetav,  
           bool expected_result )
          {
           checkMacro(  gc0,   x0,   y0,   theta0,   x1,   y1,   theta1,   xv,   yv,   thetav,   expected_result );
           checkMacro(  gc1,   x0,   y0,   theta0,   x1,   y1,   theta1,   xv,   yv,   thetav,   expected_result );
          }
          
      90  void trueFalse(  
           nav2_core::GoalChecker & gc0,   nav2_core::GoalChecker & gc1,  
           double x0,   double y0,   double theta0,  
           double x1,   double y1,   double theta1,  
           double xv,   double yv,   double thetav )
          {
           checkMacro(  gc0,   x0,   y0,   theta0,   x1,   y1,   theta1,   xv,   yv,   thetav,   true );
           checkMacro(  gc1,   x0,   y0,   theta0,   x1,   y1,   theta1,   xv,   yv,   thetav,   false );
          }
      99  class TestLifecycleNode : public nav2_util::LifecycleNode
          {
          public:
     102   explicit TestLifecycleNode(  const std::string & name )
           : nav2_util::LifecycleNode(  name )
           {
           }
          
     107   nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     112   nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     117   nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     122   nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     127   nav2_util::CallbackReturn onShutdown(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     132   nav2_util::CallbackReturn onError(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          };
          
     138  TEST(  VelocityIterator,   goal_checker_reset )
          {
           auto x = std::make_shared<TestLifecycleNode>(  "goal_checker" );
          
           nav2_core::GoalChecker * gc = new SimpleGoalChecker;
           gc->reset(   );
           delete gc;
           EXPECT_TRUE(  true );
          }
          
     148  TEST(  VelocityIterator,   two_checks )
          {
           auto x = std::make_shared<TestLifecycleNode>(  "goal_checker" );
          
           SimpleGoalChecker gc;
           StoppedGoalChecker sgc;
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           gc.initialize(  x,   "nav2_controller",   costmap );
           sgc.initialize(  x,   "nav2_controller",   costmap );
           sameResult(  gc,   sgc,   0,   0,   0,   0,   0,   0,   0,   0,   0,   true );
           sameResult(  gc,   sgc,   0,   0,   0,   1,   0,   0,   0,   0,   0,   false );
           sameResult(  gc,   sgc,   0,   0,   0,   0,   1,   0,   0,   0,   0,   false );
           sameResult(  gc,   sgc,   0,   0,   0,   0,   0,   1,   0,   0,   0,   false );
           sameResult(  gc,   sgc,   0,   0,   3.14,   0,   0,   -3.14,   0,   0,   0,   true );
           trueFalse(  gc,   sgc,   0,   0,   3.14,   0,   0,   -3.14,   1,   0,   0 );
           trueFalse(  gc,   sgc,   0,   0,   0,   0,   0,   0,   1,   0,   0 );
           trueFalse(  gc,   sgc,   0,   0,   0,   0,   0,   0,   0,   1,   0 );
           trueFalse(  gc,   sgc,   0,   0,   0,   0,   0,   0,   0,   0,   1 );
          }
          
     169  TEST(  StoppedGoalChecker,   get_tol_and_dynamic_params )
          {
           auto x = std::make_shared<TestLifecycleNode>(  "goal_checker" );
          
           SimpleGoalChecker gc;
           StoppedGoalChecker sgc;
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           sgc.initialize(  x,   "test",   costmap );
           gc.initialize(  x,   "test2",   costmap );
           geometry_msgs::msg::Pose pose_tol;
           geometry_msgs::msg::Twist vel_tol;
          
           // Test stopped goal checker's tolerance API
           EXPECT_TRUE(  sgc.getTolerances(  pose_tol,   vel_tol ) );
           EXPECT_EQ(  vel_tol.linear.x,   0.25 );
           EXPECT_EQ(  vel_tol.linear.y,   0.25 );
           EXPECT_EQ(  vel_tol.angular.z,   0.25 );
          
           // Test Stopped goal checker's dynamic parameters
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           x->get_node_base_interface(   ),   x->get_node_topics_interface(   ),  
           x->get_node_graph_interface(   ),  
           x->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.rot_stopped_velocity",   100.0 ),  
           rclcpp::Parameter(  "test.trans_stopped_velocity",   100.0 )} );
          
           rclcpp::spin_until_future_complete(  
           x->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  x->get_parameter(  "test.rot_stopped_velocity" ).as_double(   ),   100.0 );
           EXPECT_EQ(  x->get_parameter(  "test.trans_stopped_velocity" ).as_double(   ),   100.0 );
          
           // Test normal goal checker's dynamic parameters
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test2.xy_goal_tolerance",   200.0 ),  
           rclcpp::Parameter(  "test2.yaw_goal_tolerance",   200.0 ),  
           rclcpp::Parameter(  "test2.stateful",   true )} );
          
           rclcpp::spin_until_future_complete(  
           x->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  x->get_parameter(  "test2.xy_goal_tolerance" ).as_double(   ),   200.0 );
           EXPECT_EQ(  x->get_parameter(  "test2.yaw_goal_tolerance" ).as_double(   ),   200.0 );
           EXPECT_EQ(  x->get_parameter(  "test2.stateful" ).as_bool(   ),   true );
          
           // Test the dynamic parameters impacted the tolerances
           EXPECT_TRUE(  sgc.getTolerances(  pose_tol,   vel_tol ) );
           EXPECT_EQ(  vel_tol.linear.x,   100.0 );
           EXPECT_EQ(  vel_tol.linear.y,   100.0 );
           EXPECT_EQ(  vel_tol.angular.z,   100.0 );
          
           EXPECT_TRUE(  gc.getTolerances(  pose_tol,   vel_tol ) );
           EXPECT_EQ(  pose_tol.position.x,   200.0 );
           EXPECT_EQ(  pose_tol.position.y,   200.0 );
          }
          
     230  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_controller/plugins/test/progress_checker.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "nav2_controller/plugins/simple_progress_checker.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          using nav2_controller::SimpleProgressChecker;
          
      45  class TestLifecycleNode : public nav2_util::LifecycleNode
          {
          public:
      48   explicit TestLifecycleNode(  const std::string & name )
           : nav2_util::LifecycleNode(  name )
           {
           }
          
      53   nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      58   nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      63   nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      68   nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      73   nav2_util::CallbackReturn onShutdown(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      78   nav2_util::CallbackReturn onError(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          };
          
      84  void checkMacro(  
           nav2_core::ProgressChecker & pc,  
           double x0,   double y0,  
           double x1,   double y1,  
           int delay,  
           bool expected_result )
          {
           pc.reset(   );
           geometry_msgs::msg::PoseStamped pose0,   pose1;
           pose0.pose.position.x = x0;
           pose0.pose.position.y = y0;
           pose1.pose.position.x = x1;
           pose1.pose.position.y = y1;
           EXPECT_TRUE(  pc.check(  pose0 ) );
           rclcpp::sleep_for(  std::chrono::seconds(  delay ) );
           if (  expected_result ) {
           EXPECT_TRUE(  pc.check(  pose1 ) );
           } else {
           EXPECT_FALSE(  pc.check(  pose1 ) );
           }
          }
          
     106  TEST(  SimpleProgressChecker,   progress_checker_reset )
          {
           auto x = std::make_shared<TestLifecycleNode>(  "progress_checker" );
          
           nav2_core::ProgressChecker * pc = new SimpleProgressChecker;
           pc->reset(   );
           delete pc;
           EXPECT_TRUE(  true );
          }
          
     116  TEST(  SimpleProgressChecker,   unit_tests )
          {
           auto x = std::make_shared<TestLifecycleNode>(  "progress_checker" );
          
           SimpleProgressChecker pc;
           pc.initialize(  x,   "nav2_controller" );
           checkMacro(  pc,   0,   0,   0,   0,   1,   true );
           checkMacro(  pc,   0,   0,   1,   0,   1,   true );
           checkMacro(  pc,   0,   0,   0,   1,   1,   true );
           checkMacro(  pc,   0,   0,   1,   0,   11,   true );
           checkMacro(  pc,   0,   0,   0,   1,   11,   true );
           checkMacro(  pc,   0,   0,   0,   0,   11,   false );
          }
          
     130  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_controller/src/controller_server.cpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <vector>
          #include <memory>
          #include <string>
          #include <utility>
          #include <limits>
          
          #include "nav2_core/exceptions.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav_2d_utils/tf_help.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_controller/controller_server.hpp"
          
          using namespace std::chrono_literals;
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_controller
          {
          
      36  ControllerServer::ControllerServer(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "controller_server",   "",   options ),  
           progress_checker_loader_(  "nav2_core",   "nav2_core::ProgressChecker" ),  
           default_progress_checker_id_{"progress_checker"},  
      40   default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"},  
      41   goal_checker_loader_(  "nav2_core",   "nav2_core::GoalChecker" ),  
           default_goal_checker_ids_{"goal_checker"},  
           default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"},  
           lp_loader_(  "nav2_core",   "nav2_core::Controller" ),  
           default_ids_{"FollowPath"},  
           default_types_{"dwb_core::DWBLocalPlanner"}
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating controller server" );
          
           declare_parameter(  "controller_frequency",   20.0 );
          
           declare_parameter(  "progress_checker_plugin",   default_progress_checker_id_ );
           declare_parameter(  "goal_checker_plugins",   default_goal_checker_ids_ );
           declare_parameter(  "controller_plugins",   default_ids_ );
           declare_parameter(  "min_x_velocity_threshold",   rclcpp::ParameterValue(  0.0001 ) );
           declare_parameter(  "min_y_velocity_threshold",   rclcpp::ParameterValue(  0.0001 ) );
           declare_parameter(  "min_theta_velocity_threshold",   rclcpp::ParameterValue(  0.0001 ) );
          
           declare_parameter(  "speed_limit_topic",   rclcpp::ParameterValue(  "speed_limit" ) );
          
           declare_parameter(  "failure_tolerance",   rclcpp::ParameterValue(  0.0 ) );
          
           // The costmap node is used in the implementation of the controller
           costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  
           "local_costmap",   std::string{get_namespace(   )},   "local_costmap" );
          
           // Launch a thread to run the costmap node
           costmap_thread_ = std::make_unique<nav2_util::NodeThread>(  costmap_ros_ );
          }
          
          ControllerServer::~ControllerServer(   )
          {
           progress_checker_.reset(   );
           goal_checkers_.clear(   );
           controllers_.clear(   );
           costmap_thread_.reset(   );
          }
          
          nav2_util::CallbackReturn
          ControllerServer::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           auto node = shared_from_this(   );
          
           RCLCPP_INFO(  get_logger(   ),   "Configuring controller interface" );
          
           get_parameter(  "progress_checker_plugin",   progress_checker_id_ );
           if (  progress_checker_id_ == default_progress_checker_id_ ) {
           nav2_util::declare_parameter_if_not_declared(  
           node,   default_progress_checker_id_ + ".plugin",  
           rclcpp::ParameterValue(  default_progress_checker_type_ ) );
           }
          
           RCLCPP_INFO(  get_logger(   ),   "getting goal checker plugins.." );
           get_parameter(  "goal_checker_plugins",   goal_checker_ids_ );
           if (  goal_checker_ids_ == default_goal_checker_ids_ ) {
           for (  size_t i = 0; i < default_goal_checker_ids_.size(   ); ++i ) {
           nav2_util::declare_parameter_if_not_declared(  
           node,   default_goal_checker_ids_[i] + ".plugin",  
           rclcpp::ParameterValue(  default_goal_checker_types_[i] ) );
           }
           }
          
           get_parameter(  "controller_plugins",   controller_ids_ );
           if (  controller_ids_ == default_ids_ ) {
           for (  size_t i = 0; i < default_ids_.size(   ); ++i ) {
           nav2_util::declare_parameter_if_not_declared(  
           node,   default_ids_[i] + ".plugin",  
           rclcpp::ParameterValue(  default_types_[i] ) );
           }
           }
          
           controller_types_.resize(  controller_ids_.size(   ) );
           goal_checker_types_.resize(  goal_checker_ids_.size(   ) );
          
           get_parameter(  "controller_frequency",   controller_frequency_ );
           get_parameter(  "min_x_velocity_threshold",   min_x_velocity_threshold_ );
           get_parameter(  "min_y_velocity_threshold",   min_y_velocity_threshold_ );
           get_parameter(  "min_theta_velocity_threshold",   min_theta_velocity_threshold_ );
           RCLCPP_INFO(  get_logger(   ),   "Controller frequency set to %.4fHz",   controller_frequency_ );
          
           std::string speed_limit_topic;
           get_parameter(  "speed_limit_topic",   speed_limit_topic );
           get_parameter(  "failure_tolerance",   failure_tolerance_ );
          
           costmap_ros_->configure(   );
          
           try {
           progress_checker_type_ = nav2_util::get_plugin_type_param(  node,   progress_checker_id_ );
           progress_checker_ = progress_checker_loader_.createUniqueInstance(  progress_checker_type_ );
           RCLCPP_INFO(  
           get_logger(   ),   "Created progress_checker : %s of type %s",  
           progress_checker_id_.c_str(   ),   progress_checker_type_.c_str(   ) );
           progress_checker_->initialize(  node,   progress_checker_id_ );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Failed to create progress_checker. Exception: %s",   ex.what(   ) );
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           for (  size_t i = 0; i != goal_checker_ids_.size(   ); i++ ) {
           try {
           goal_checker_types_[i] = nav2_util::get_plugin_type_param(  node,   goal_checker_ids_[i] );
           nav2_core::GoalChecker::Ptr goal_checker =
           goal_checker_loader_.createUniqueInstance(  goal_checker_types_[i] );
           RCLCPP_INFO(  
           get_logger(   ),   "Created goal checker : %s of type %s",  
           goal_checker_ids_[i].c_str(   ),   goal_checker_types_[i].c_str(   ) );
           goal_checker->initialize(  node,   goal_checker_ids_[i],   costmap_ros_ );
           goal_checkers_.insert(  {goal_checker_ids_[i],   goal_checker} );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Failed to create goal checker. Exception: %s",   ex.what(   ) );
           return nav2_util::CallbackReturn::FAILURE;
           }
           }
          
           for (  size_t i = 0; i != goal_checker_ids_.size(   ); i++ ) {
           goal_checker_ids_concat_ += goal_checker_ids_[i] + std::string(  " " );
           }
          
           RCLCPP_INFO(  
           get_logger(   ),  
           "Controller Server has %s goal checkers available.",   goal_checker_ids_concat_.c_str(   ) );
          
           for (  size_t i = 0; i != controller_ids_.size(   ); i++ ) {
           try {
           controller_types_[i] = nav2_util::get_plugin_type_param(  node,   controller_ids_[i] );
           nav2_core::Controller::Ptr controller =
           lp_loader_.createUniqueInstance(  controller_types_[i] );
           RCLCPP_INFO(  
           get_logger(   ),   "Created controller : %s of type %s",  
           controller_ids_[i].c_str(   ),   controller_types_[i].c_str(   ) );
           controller->configure(  
           node,   controller_ids_[i],  
           costmap_ros_->getTfBuffer(   ),   costmap_ros_ );
           controllers_.insert(  {controller_ids_[i],   controller} );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Failed to create controller. Exception: %s",   ex.what(   ) );
           return nav2_util::CallbackReturn::FAILURE;
           }
           }
          
           for (  size_t i = 0; i != controller_ids_.size(   ); i++ ) {
           controller_ids_concat_ += controller_ids_[i] + std::string(  " " );
           }
          
           RCLCPP_INFO(  
           get_logger(   ),  
           "Controller Server has %s controllers available.",   controller_ids_concat_.c_str(   ) );
          
           odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(  node );
           vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>(  "cmd_vel",   1 );
          
           // Create the action server that we implement with our followPath method
           action_server_ = std::make_unique<ActionServer>(  
           shared_from_this(   ),  
           "follow_path",  
           std::bind(  &ControllerServer::computeControl,   this ),  
           nullptr,  
           std::chrono::milliseconds(  500 ),  
           true );
          
           // Set subscribtion to the speed limiting topic
           speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(  
           speed_limit_topic,   rclcpp::QoS(  10 ),  
           std::bind(  &ControllerServer::speedLimitCallback,   this,   std::placeholders::_1 ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          ControllerServer::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           costmap_ros_->activate(   );
           ControllerMap::iterator it;
           for (  it = controllers_.begin(   ); it != controllers_.end(   ); ++it ) {
           it->second->activate(   );
           }
           vel_publisher_->on_activate(   );
           action_server_->activate(   );
          
           auto node = shared_from_this(   );
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &ControllerServer::dynamicParametersCallback,   this,   _1 ) );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          ControllerServer::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           action_server_->deactivate(   );
           ControllerMap::iterator it;
           for (  it = controllers_.begin(   ); it != controllers_.end(   ); ++it ) {
           it->second->deactivate(   );
           }
           costmap_ros_->deactivate(   );
          
           publishZeroVelocity(   );
           vel_publisher_->on_deactivate(   );
           dyn_params_handler_.reset(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          ControllerServer::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           // Cleanup the helper classes
           ControllerMap::iterator it;
           for (  it = controllers_.begin(   ); it != controllers_.end(   ); ++it ) {
           it->second->cleanup(   );
           }
           controllers_.clear(   );
          
           goal_checkers_.clear(   );
           costmap_ros_->cleanup(   );
          
           // Release any allocated resources
           action_server_.reset(   );
           odom_sub_.reset(   );
           vel_publisher_.reset(   );
           speed_limit_sub_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          ControllerServer::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          bool ControllerServer::findControllerId(  
           const std::string & c_name,  
           std::string & current_controller )
          {
           if (  controllers_.find(  c_name ) == controllers_.end(   ) ) {
           if (  controllers_.size(   ) == 1 && c_name.empty(   ) ) {
           RCLCPP_WARN_ONCE(  
           get_logger(   ),   "No controller was specified in action call."
           " Server will use only plugin loaded %s. "
           "This warning will appear once.",   controller_ids_concat_.c_str(   ) );
           current_controller = controllers_.begin(   )->first;
           } else {
           RCLCPP_ERROR(  
           get_logger(   ),   "FollowPath called with controller name %s,   "
           "which does not exist. Available controllers are: %s.",  
           c_name.c_str(   ),   controller_ids_concat_.c_str(   ) );
           return false;
           }
           } else {
           RCLCPP_DEBUG(  get_logger(   ),   "Selected controller: %s.",   c_name.c_str(   ) );
           current_controller = c_name;
           }
          
           return true;
          }
          
          bool ControllerServer::findGoalCheckerId(  
           const std::string & c_name,  
           std::string & current_goal_checker )
          {
           if (  goal_checkers_.find(  c_name ) == goal_checkers_.end(   ) ) {
           if (  goal_checkers_.size(   ) == 1 && c_name.empty(   ) ) {
           RCLCPP_WARN_ONCE(  
           get_logger(   ),   "No goal checker was specified in parameter 'current_goal_checker'."
           " Server will use only plugin loaded %s. "
           "This warning will appear once.",   goal_checker_ids_concat_.c_str(   ) );
           current_goal_checker = goal_checkers_.begin(   )->first;
           } else {
           RCLCPP_ERROR(  
           get_logger(   ),   "FollowPath called with goal_checker name %s in parameter"
           " 'current_goal_checker',   which does not exist. Available goal checkers are: %s.",  
           c_name.c_str(   ),   goal_checker_ids_concat_.c_str(   ) );
           return false;
           }
           } else {
           RCLCPP_DEBUG(  get_logger(   ),   "Selected goal checker: %s.",   c_name.c_str(   ) );
           current_goal_checker = c_name;
           }
          
           return true;
          }
          
          void ControllerServer::computeControl(   )
          {
           std::lock_guard<std::mutex> lock(  dynamic_params_lock_ );
          
           RCLCPP_INFO(  get_logger(   ),   "Received a goal,   begin computing control effort." );
          
           try {
           std::string c_name = action_server_->get_current_goal(   )->controller_id;
           std::string current_controller;
           if (  findControllerId(  c_name,   current_controller ) ) {
           current_controller_ = current_controller;
           } else {
           action_server_->terminate_current(   );
           return;
           }
          
           std::string gc_name = action_server_->get_current_goal(   )->goal_checker_id;
           std::string current_goal_checker;
           if (  findGoalCheckerId(  gc_name,   current_goal_checker ) ) {
           current_goal_checker_ = current_goal_checker;
           } else {
           action_server_->terminate_current(   );
           return;
           }
          
           setPlannerPath(  action_server_->get_current_goal(   )->path );
           progress_checker_->reset(   );
          
           last_valid_cmd_time_ = now(   );
           rclcpp::WallRate loop_rate(  controller_frequency_ );
           while (  rclcpp::ok(   ) ) {
           if (  action_server_ == nullptr || !action_server_->is_server_active(   ) ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Action server unavailable or inactive. Stopping." );
           return;
           }
          
           if (  action_server_->is_cancel_requested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Goal was canceled. Stopping the robot." );
           action_server_->terminate_all(   );
           publishZeroVelocity(   );
           return;
           }
          
           // Don't compute a trajectory until costmap is valid (  after clear costmap )
           rclcpp::Rate r(  100 );
           while (  !costmap_ros_->isCurrent(   ) ) {
           r.sleep(   );
           }
          
           updateGlobalPath(   );
          
           computeAndPublishVelocity(   );
          
           if (  isGoalReached(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Reached the goal!" );
           break;
           }
          
           if (  !loop_rate.sleep(   ) ) {
           RCLCPP_WARN(  
           get_logger(   ),   "Control loop missed its desired rate of %.4fHz",  
           controller_frequency_ );
           }
           }
           } catch (  nav2_core::PlannerException & e ) {
           RCLCPP_ERROR(  this->get_logger(   ),   e.what(   ) );
           publishZeroVelocity(   );
           action_server_->terminate_current(   );
           return;
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "Controller succeeded,   setting result" );
          
           publishZeroVelocity(   );
          
           // TODO(  orduno ) #861 Handle a pending preemption and set controller name
           action_server_->succeeded_current(   );
          }
          
          void ControllerServer::setPlannerPath(  const nav_msgs::msg::Path & path )
          {
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Providing path to the controller %s",   current_controller_.c_str(   ) );
           if (  path.poses.empty(   ) ) {
           throw nav2_core::PlannerException(  "Invalid path,   Path is empty." );
           }
           controllers_[current_controller_]->setPlan(  path );
          
           end_pose_ = path.poses.back(   );
           end_pose_.header.frame_id = path.header.frame_id;
           goal_checkers_[current_goal_checker_]->reset(   );
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "Path end point is (  %.2f,   %.2f )",  
           end_pose_.pose.position.x,   end_pose_.pose.position.y );
          
           current_path_ = path;
          }
          
          void ControllerServer::computeAndPublishVelocity(   )
          {
           geometry_msgs::msg::PoseStamped pose;
          
           if (  !getRobotPose(  pose ) ) {
           throw nav2_core::PlannerException(  "Failed to obtain robot pose" );
           }
          
           if (  !progress_checker_->check(  pose ) ) {
           throw nav2_core::PlannerException(  "Failed to make progress" );
           }
          
           nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(  odom_sub_->getTwist(   ) );
          
           geometry_msgs::msg::TwistStamped cmd_vel_2d;
          
           try {
           cmd_vel_2d =
           controllers_[current_controller_]->computeVelocityCommands(  
           pose,  
           nav_2d_utils::twist2Dto3D(  twist ),  
           goal_checkers_[current_goal_checker_].get(   ) );
           last_valid_cmd_time_ = now(   );
           } catch (  nav2_core::PlannerException & e ) {
           if (  failure_tolerance_ > 0 || failure_tolerance_ == -1.0 ) {
           RCLCPP_WARN(  this->get_logger(   ),   e.what(   ) );
           cmd_vel_2d.twist.angular.x = 0;
           cmd_vel_2d.twist.angular.y = 0;
           cmd_vel_2d.twist.angular.z = 0;
           cmd_vel_2d.twist.linear.x = 0;
           cmd_vel_2d.twist.linear.y = 0;
           cmd_vel_2d.twist.linear.z = 0;
           cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(   );
           cmd_vel_2d.header.stamp = now(   );
           if (  (  now(   ) - last_valid_cmd_time_ ).seconds(   ) > failure_tolerance_ &&
           failure_tolerance_ != -1.0 )
           {
           throw nav2_core::PlannerException(  "Controller patience exceeded" );
           }
           } else {
           throw nav2_core::PlannerException(  e.what(   ) );
           }
           }
          
           std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>(   );
           feedback->speed = std::hypot(  cmd_vel_2d.twist.linear.x,   cmd_vel_2d.twist.linear.y );
          
           // Find the closest pose to current pose on global path
           nav_msgs::msg::Path & current_path = current_path_;
           auto find_closest_pose_idx =
           [&pose,   &current_path](   ) {
           size_t closest_pose_idx = 0;
           double curr_min_dist = std::numeric_limits<double>::max(   );
           for (  size_t curr_idx = 0; curr_idx < current_path.poses.size(   ); ++curr_idx ) {
           double curr_dist = nav2_util::geometry_utils::euclidean_distance(  
           pose,   current_path.poses[curr_idx] );
           if (  curr_dist < curr_min_dist ) {
           curr_min_dist = curr_dist;
           closest_pose_idx = curr_idx;
           }
           }
           return closest_pose_idx;
           };
          
           feedback->distance_to_goal =
           nav2_util::geometry_utils::calculate_path_length(  current_path_,   find_closest_pose_idx(   ) );
           action_server_->publish_feedback(  feedback );
          
           RCLCPP_DEBUG(  get_logger(   ),   "Publishing velocity at time %.2f",   now(   ).seconds(   ) );
           publishVelocity(  cmd_vel_2d );
          }
          
          void ControllerServer::updateGlobalPath(   )
          {
           if (  action_server_->is_preempt_requested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Passing new path to controller." );
           auto goal = action_server_->accept_pending_goal(   );
           std::string current_controller;
           if (  findControllerId(  goal->controller_id,   current_controller ) ) {
           current_controller_ = current_controller;
           } else {
           RCLCPP_INFO(  
           get_logger(   ),   "Terminating action,   invalid controller %s requested.",  
           goal->controller_id.c_str(   ) );
           action_server_->terminate_current(   );
           return;
           }
           setPlannerPath(  goal->path );
           }
          }
          
          void ControllerServer::publishVelocity(  const geometry_msgs::msg::TwistStamped & velocity )
          {
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(  velocity.twist );
           if (  vel_publisher_->is_activated(   ) && vel_publisher_->get_subscription_count(   ) > 0 ) {
           vel_publisher_->publish(  std::move(  cmd_vel ) );
           }
          }
          
          void ControllerServer::publishZeroVelocity(   )
          {
           geometry_msgs::msg::TwistStamped velocity;
           velocity.twist.angular.x = 0;
           velocity.twist.angular.y = 0;
           velocity.twist.angular.z = 0;
           velocity.twist.linear.x = 0;
           velocity.twist.linear.y = 0;
           velocity.twist.linear.z = 0;
           velocity.header.frame_id = costmap_ros_->getBaseFrameID(   );
           velocity.header.stamp = now(   );
           publishVelocity(  velocity );
          }
          
          bool ControllerServer::isGoalReached(   )
          {
           geometry_msgs::msg::PoseStamped pose;
          
           if (  !getRobotPose(  pose ) ) {
           return false;
           }
          
           nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(  odom_sub_->getTwist(   ) );
           geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(  twist );
          
           geometry_msgs::msg::PoseStamped transformed_end_pose;
           rclcpp::Duration tolerance(  rclcpp::Duration::from_seconds(  costmap_ros_->getTransformTolerance(   ) ) );
           nav_2d_utils::transformPose(  
           costmap_ros_->getTfBuffer(   ),   costmap_ros_->getGlobalFrameID(   ),  
           end_pose_,   transformed_end_pose,   tolerance );
          
           return goal_checkers_[current_goal_checker_]->isGoalReached(  
           pose.pose,   transformed_end_pose.pose,  
           velocity );
          }
          
          bool ControllerServer::getRobotPose(  geometry_msgs::msg::PoseStamped & pose )
          {
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !costmap_ros_->getRobotPose(  current_pose ) ) {
           return false;
           }
           pose = current_pose;
           return true;
          }
          
          void ControllerServer::speedLimitCallback(  const nav2_msgs::msg::SpeedLimit::SharedPtr msg )
          {
           ControllerMap::iterator it;
           for (  it = controllers_.begin(   ); it != controllers_.end(   ); ++it ) {
           it->second->setSpeedLimit(  msg->speed_limit,   msg->percentage );
           }
          }
          
          rcl_interfaces::msg::SetParametersResult
          ControllerServer::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           // If we are trying to change the parameter of a plugin we can just skip it at this point
           // as they handle parameter changes themselves and don't need to lock the mutex
           if (  name.find(  '.' ) != std::string::npos ) {
           continue;
           }
          
           if (  !dynamic_params_lock_.try_lock(   ) ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Unable to dynamically change Parameters while the controller is currently running" );
           result.successful = false;
           result.reason =
           "Unable to dynamically change Parameters while the controller is currently running";
           return result;
           }
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == "controller_frequency" ) {
           controller_frequency_ = parameter.as_double(   );
           } else if (  name == "min_x_velocity_threshold" ) {
           min_x_velocity_threshold_ = parameter.as_double(   );
           } else if (  name == "min_y_velocity_threshold" ) {
           min_y_velocity_threshold_ = parameter.as_double(   );
           } else if (  name == "min_theta_velocity_threshold" ) {
           min_theta_velocity_threshold_ = parameter.as_double(   );
           } else if (  name == "failure_tolerance" ) {
           failure_tolerance_ = parameter.as_double(   );
           }
           }
          
           dynamic_params_lock_.unlock(   );
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_controller
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
          RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_controller::ControllerServer )

navigation2/nav2_controller/src/main.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_controller/controller_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_controller::ControllerServer>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_controller/test/test_dynamic_parameters.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_controller/controller_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      26  class ControllerShim : public nav2_controller::ControllerServer
          {
          public:
      29   ControllerShim(   )
           : nav2_controller::ControllerServer(  rclcpp::NodeOptions(   ) )
           {
           }
          
           // Since we cannot call configure/activate due to costmaps
           // requiring TF
      36   void setDynamicCallback(   )
           {
           auto node = shared_from_this(   );
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &ControllerShim::dynamicParamsShim,   this,   std::placeholders::_1 ) );
           }
          
           rcl_interfaces::msg::SetParametersResult
      45   dynamicParamsShim(  std::vector<rclcpp::Parameter> parameters )
           {
           rcl_interfaces::msg::SetParametersResult result;
           result.successful = true;
           dynamicParametersCallback(  parameters );
           return result;
           }
          };
          
      54  class RclCppFixture
          {
          public:
      57   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      58   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      62  TEST(  WPTest,   test_dynamic_parameters )
          {
           auto controller = std::make_shared<ControllerShim>(   );
           controller->setDynamicCallback(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           controller->get_node_base_interface(   ),   controller->get_node_topics_interface(   ),  
           controller->get_node_graph_interface(   ),  
           controller->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "controller_frequency",   100.0 ),  
           rclcpp::Parameter(  "min_x_velocity_threshold",   100.0 ),  
           rclcpp::Parameter(  "min_y_velocity_threshold",   100.0 ),  
           rclcpp::Parameter(  "min_theta_velocity_threshold",   100.0 ),  
           rclcpp::Parameter(  "failure_tolerance",   5.0 )} );
          
           rclcpp::spin_until_future_complete(  
           controller->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  controller->get_parameter(  "controller_frequency" ).as_double(   ),   100.0 );
           EXPECT_EQ(  controller->get_parameter(  "min_x_velocity_threshold" ).as_double(   ),   100.0 );
           EXPECT_EQ(  controller->get_parameter(  "min_y_velocity_threshold" ).as_double(   ),   100.0 );
           EXPECT_EQ(  controller->get_parameter(  "min_theta_velocity_threshold" ).as_double(   ),   100.0 );
           EXPECT_EQ(  controller->get_parameter(  "failure_tolerance" ).as_double(   ),   5.0 );
          }

navigation2/nav2_core/include/nav2_core/behavior.hpp

          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CORE__BEHAVIOR_HPP_
          #define NAV2_CORE__BEHAVIOR_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "tf2_ros/buffer.h"
          #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
          
          namespace nav2_core
          {
          
          /**
           * @class Behavior
           * @brief Abstract interface for behaviors to adhere to with pluginlib
           */
      33  class Behavior
          {
          public:
           using Ptr = std::shared_ptr<Behavior>;
          
           /**
           * @brief Virtual destructor
           */
           virtual ~Behavior(   ) {}
          
           /**
           * @param parent pointer to user's node
           * @param name The name of this planner
           * @param tf A pointer to a TF buffer
           * @param costmap_ros A pointer to the costmap
           */
           virtual void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           const std::string & name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker ) = 0;
          
           /**
           * @brief Method to cleanup resources used on shutdown.
           */
      57   virtual void cleanup(   ) = 0;
          
           /**
           * @brief Method to active Behavior and any threads involved in execution.
           */
           virtual void activate(   ) = 0;
          
           /**
           * @brief Method to deactive Behavior and any threads involved in execution.
           */
           virtual void deactivate(   ) = 0;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__BEHAVIOR_HPP_

navigation2/nav2_core/include/nav2_core/controller.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * Copyright (  c ) 2019,   Intel Corporation
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_CORE__CONTROLLER_HPP_
          #define NAV2_CORE__CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/twist_stamped.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_core/goal_checker.hpp"
          
          
          namespace nav2_core
          {
          
          /**
           * @class Controller
           * @brief controller interface that acts as a virtual base class for all controller plugins
           */
      60  class Controller
          {
          public:
           using Ptr = std::shared_ptr<nav2_core::Controller>;
          
          
           /**
           * @brief Virtual destructor
           */
           virtual ~Controller(   ) {}
          
           /**
           * @param parent pointer to user's node
           * @param costmap_ros A pointer to the costmap
           */
           virtual void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr &,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer>,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> ) = 0;
          
           /**
           * @brief Method to cleanup resources.
           */
      83   virtual void cleanup(   ) = 0;
          
           /**
           * @brief Method to active planner and any threads involved in execution.
           */
           virtual void activate(   ) = 0;
          
           /**
           * @brief Method to deactive planner and any threads involved in execution.
           */
           virtual void deactivate(   ) = 0;
          
           /**
           * @brief local setPlan - Sets the global plan
           * @param path The global plan
           */
           virtual void setPlan(  const nav_msgs::msg::Path & path ) = 0;
          
           /**
           * @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity
           *
           * It is presumed that the global plan is already set.
           *
           * This is mostly a wrapper for the protected computeVelocityCommands
           * function which has additional debugging info.
           *
           * @param pose Current robot pose
           * @param velocity Current robot velocity
           * @param goal_checker Pointer to the current goal checker the task is utilizing
           * @return The best command for the robot to drive
           */
           virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(  
           const geometry_msgs::msg::PoseStamped & pose,  
           const geometry_msgs::msg::Twist & velocity,  
           nav2_core::GoalChecker * goal_checker ) = 0;
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
           virtual void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) = 0;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__CONTROLLER_HPP_

navigation2/nav2_core/include/nav2_core/exceptions.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * Copyright (  c ) 2019,   Intel Corporation
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_CORE__EXCEPTIONS_HPP_
          #define NAV2_CORE__EXCEPTIONS_HPP_
          
          #include <stdexcept>
          #include <string>
          #include <memory>
          
          namespace nav2_core
          {
          
      46  class PlannerException : public std::runtime_error
          {
          public:
      49   explicit PlannerException(  const std::string description )
           : std::runtime_error(  description ) {}
           using Ptr = std::shared_ptr<PlannerException>;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__EXCEPTIONS_HPP_

navigation2/nav2_core/include/nav2_core/global_planner.hpp

          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CORE__GLOBAL_PLANNER_HPP_
          #define NAV2_CORE__GLOBAL_PLANNER_HPP_
          
          #include <memory>
          #include <string>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "tf2_ros/buffer.h"
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace nav2_core
          {
          
          /**
           * @class GlobalPlanner
           * @brief Abstract interface for global planners to adhere to with pluginlib
           */
      34  class GlobalPlanner
          {
          public:
           using Ptr = std::shared_ptr<GlobalPlanner>;
          
           /**
           * @brief Virtual destructor
           */
           virtual ~GlobalPlanner(   ) {}
          
           /**
           * @param parent pointer to user's node
           * @param name The name of this planner
           * @param tf A pointer to a TF buffer
           * @param costmap_ros A pointer to the costmap
           */
           virtual void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) = 0;
          
           /**
           * @brief Method to cleanup resources used on shutdown.
           */
      58   virtual void cleanup(   ) = 0;
          
           /**
           * @brief Method to active planner and any threads involved in execution.
           */
           virtual void activate(   ) = 0;
          
           /**
           * @brief Method to deactive planner and any threads involved in execution.
           */
           virtual void deactivate(   ) = 0;
          
           /**
           * @brief Method create the plan from a starting and ending goal.
           * @param start The starting pose of the robot
           * @param goal The goal pose of the robot
           * @return The sequence of poses to get from start to goal,   if any
           */
           virtual nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) = 0;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__GLOBAL_PLANNER_HPP_

navigation2/nav2_core/include/nav2_core/goal_checker.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_CORE__GOAL_CHECKER_HPP_
          #define NAV2_CORE__GOAL_CHECKER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "geometry_msgs/msg/pose.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          
          namespace nav2_core
          {
          
          /**
           * @class GoalChecker
           * @brief Function-object for checking whether a goal has been reached
           *
           * This class defines the plugin interface for determining whether you have reached
           * the goal state. This primarily consists of checking the relative positions of two poses
           * (  which are presumed to be in the same frame ). It can also check the velocity,   as some
           * applications require that robot be stopped to be considered as having reached the goal.
           */
      61  class GoalChecker
          {
          public:
           typedef std::shared_ptr<nav2_core::GoalChecker> Ptr;
          
      66   virtual ~GoalChecker(   ) {}
          
           /**
           * @brief Initialize any parameters from the NodeHandle
           * @param parent Node pointer for grabbing parameters
           */
      72   virtual void initialize(  
      73   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      74   const std::string & plugin_name,  
      75   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) = 0;
          
      77   virtual void reset(   ) = 0;
          
           /**
           * @brief Check whether the goal should be considered reached
           * @param query_pose The pose to check
           * @param goal_pose The pose to check against
           * @param velocity The robot's current velocity
           * @return True if goal is reached
           */
      86   virtual bool isGoalReached(  
      87   const geometry_msgs::msg::Pose & query_pose,   const geometry_msgs::msg::Pose & goal_pose,  
      88   const geometry_msgs::msg::Twist & velocity ) = 0;
          
           /**
           * @brief Get the maximum possible tolerances used for goal checking in the major types.
           * Any field without a valid entry is replaced with std::numeric_limits<double>::lowest(   )
           * to indicate that it is not measured. For tolerance across multiple entries
           * (  e.x. XY tolerances ),   both fields will contain this value since it is the maximum tolerance
           * that each independent field could be assuming the other has no error (  e.x. X and Y ).
           * @param pose_tolerance The tolerance used for checking in Pose fields
           * @param vel_tolerance The tolerance used for checking velocity fields
           * @return True if the tolerances are valid to use
           */
     100   virtual bool getTolerances(  
     101   geometry_msgs::msg::Pose & pose_tolerance,  
     102   geometry_msgs::msg::Twist & vel_tolerance ) = 0;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__GOAL_CHECKER_HPP_

navigation2/nav2_core/include/nav2_core/progress_checker.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_
          #define NAV2_CORE__PROGRESS_CHECKER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          
          namespace nav2_core
          {
          /**
           * @class nav2_core::ProgressChecker
           * @brief This class defines the plugin interface used to check the
           * position of the robot to make sure that it is actually progressing
           * towards a goal.
           */
      34  class ProgressChecker
          {
          public:
           typedef std::shared_ptr<nav2_core::ProgressChecker> Ptr;
          
      39   virtual ~ProgressChecker(   ) {}
          
           /**
           * @brief Initialize parameters for ProgressChecker
           * @param parent Node pointer
           */
      45   virtual void initialize(  
      46   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      47   const std::string & plugin_name ) = 0;
           /**
           * @brief Checks if the robot has moved compare to previous
           * pose
           * @param current_pose Current pose of the robot
           * @return True if progress is made
           */
      54   virtual bool check(  geometry_msgs::msg::PoseStamped & current_pose ) = 0;
           /**
           * @brief Reset class state upon calling
           */
      58   virtual void reset(   ) = 0;
          };
          } // namespace nav2_core
          
          #endif // NAV2_CORE__PROGRESS_CHECKER_HPP_

navigation2/nav2_core/include/nav2_core/smoother.hpp

          // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_CORE__SMOOTHER_HPP_
          #define NAV2_CORE__SMOOTHER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_costmap_2d/footprint_subscriber.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "nav_msgs/msg/path.hpp"
          
          
          namespace nav2_core
          {
          
          /**
           * @class Smoother
           * @brief smoother interface that acts as a virtual base class for all smoother plugins
           */
      38  class Smoother
          {
          public:
           using Ptr = std::shared_ptr<nav2_core::Smoother>;
          
           /**
           * @brief Virtual destructor
           */
           virtual ~Smoother(   ) {}
          
           virtual void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr &,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer>,  
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,  
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) = 0;
          
           /**
           * @brief Method to cleanup resources.
           */
      57   virtual void cleanup(   ) = 0;
          
           /**
           * @brief Method to activate smoother and any threads involved in execution.
           */
           virtual void activate(   ) = 0;
          
           /**
           * @brief Method to deactivate smoother and any threads involved in execution.
           */
           virtual void deactivate(   ) = 0;
          
           /**
           * @brief Method to smooth given path
           *
           * @param path In-out path to be smoothed
           * @param max_time Maximum duration smoothing should take
           * @return If smoothing was completed (  true ) or interrupted by time limit (  false )
           */
           virtual bool smooth(  
           nav_msgs::msg::Path & path,  
           const rclcpp::Duration & max_time ) = 0;
          };
          
          } // namespace nav2_core
          
          #endif // NAV2_CORE__SMOOTHER_HPP_

navigation2/nav2_core/include/nav2_core/waypoint_task_executor.hpp

       1  // Copyright (  c ) 2020 Fetullah Atas
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          
          #ifndef NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_
          #define NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_
          #pragma once
          
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          namespace nav2_core
          {
          /**
           * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals.
           *
           */
      32  class WaypointTaskExecutor
          {
          public:
           /**
           * @brief Construct a new Simple Task Execution At Waypoint Base object
           *
           */
      39   WaypointTaskExecutor(   ) {}
          
           /**
           * @brief Destroy the Simple Task Execution At Waypoint Base object
           *
           */
      45   virtual ~WaypointTaskExecutor(   ) {}
          
           /**
           * @brief Override this to setup your pub,   sub or any ros services that you will use in the plugin.
           *
           * @param parent parent node that plugin will be created within(  for an example see nav_waypoint_follower )
           * @param plugin_name plugin name comes from parameters in yaml file
           */
      53   virtual void initialize(  
      54   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      55   const std::string & plugin_name ) = 0;
          
           /**
           * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
           *
           * @param curr_pose current pose of the robot
           * @param curr_waypoint_index current waypoint,   that robot just arrived
           * @return true if task execution was successful
           * @return false if task execution failed
           */
      65   virtual bool processAtWaypoint(  
      66   const geometry_msgs::msg::PoseStamped & curr_pose,   const int & curr_waypoint_index ) = 0;
          };
          } // namespace nav2_core
          #endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * author: Dave Hershberger
           */
          #ifndef NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_
          #define NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_
          
          #include <vector>
          #include <string>
          
          namespace nav2_costmap_2d
          {
          
          /** @brief Parse a vector of vectors of floats from a string.
           * @param error_return If no error,   error_return is set to "". If
           * error,   error_return will describe the error.
           * Syntax is [[1.0,   2.0],   [3.3,   4.4,   5.5],   ...]
           *
           * On error,   error_return is set and the return value could be
           * anything,   like part of a successful parse. */
      47  std::vector<std::vector<float>> parseVVF(  const std::string & input,   std::string & error_return );
          
          } // end namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
          #define NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
          
          #include <vector>
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_msgs/srv/clear_costmap_except_region.hpp"
          #include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
          #include "nav2_msgs/srv/clear_entire_costmap.hpp"
          #include "nav2_costmap_2d/costmap_layer.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace nav2_costmap_2d
          {
          
      32  class Costmap2DROS;
          
          /**
           * @class ClearCostmapService
           * @brief Exposes services to clear costmap objects in inclusive/exclusive regions or completely
           */
      38  class ClearCostmapService
          {
          public:
           /**
           * @brief A constructor
           */
      44   ClearCostmapService(  const nav2_util::LifecycleNode::WeakPtr & parent,   Costmap2DROS & costmap );
          
           /**
           * @brief A constructor
           */
           ClearCostmapService(   ) = delete;
          
           /**
           * @brief Clears the region outside of a user-specified area reverting to the static map
           */
           void clearRegion(  double reset_distance,   bool invert );
          
           /**
           * @brief Clears all layers
           */
           void clearEntirely(   );
          
          private:
           // The Logger object for logging
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_costmap_2d" )};
          
           // The costmap to clear
           Costmap2DROS & costmap_;
          
           // Clearing parameters
           unsigned char reset_value_;
           std::vector<std::string> clearable_layers_;
          
           // Server for clearing the costmap
           rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;
           /**
           * @brief Callback to clear costmap except in a given region
           */
      77   void clearExceptRegionCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,  
           const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response );
          
           rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
           /**
           * @brief Callback to clear costmap in a given region
           */
      86   void clearAroundRobotCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,  
           const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response );
          
           rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
           /**
           * @brief Callback to clear costmap
           */
      95   void clearEntireCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request> request,  
           const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response );
          
           /**
           * @brief Function used to clear a given costmap layer
           */
     103   void clearLayerRegion(  
           std::shared_ptr<CostmapLayer> & costmap,   double pose_x,   double pose_y,   double reset_distance,  
           bool invert );
          
           /**
           * @brief Get the robot's position in the costmap using the master costmap
           */
     110   bool getPosition(  double & x,   double & y ) const;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COST_VALUES_HPP_
          #define NAV2_COSTMAP_2D__COST_VALUES_HPP_
          /** Provides a mapping for often used cost values */
          namespace nav2_costmap_2d
          {
      42  static constexpr unsigned char NO_INFORMATION = 255;
      43  static constexpr unsigned char LETHAL_OBSTACLE = 254;
      44  static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
      45  static constexpr unsigned char MAX_NON_OBSTACLE = 252;
      46  static constexpr unsigned char FREE_SPACE = 0;
          }
          #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COSTMAP_2D_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_2D_HPP_
          
          #include <string.h>
          #include <stdio.h>
          #include <limits.h>
          #include <algorithm>
          #include <cmath>
          #include <string>
          #include <vector>
          #include <queue>
          #include <mutex>
          #include "geometry_msgs/msg/point.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          
          namespace nav2_costmap_2d
          {
          
          // convenient for storing x/y point pairs
          struct MapLocation
          {
           unsigned int x;
           unsigned int y;
          };
          
          /**
           * @class Costmap2D
           * @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
           */
      67  class Costmap2D
          {
      69   friend class CostmapTester; // Need this for gtest to work correctly
          
          public:
           /**
           * @brief Constructor for a costmap
           * @param cells_size_x The x size of the map in cells
           * @param cells_size_y The y size of the map in cells
           * @param resolution The resolution of the map in meters/cell
           * @param origin_x The x origin of the map
           * @param origin_y The y origin of the map
           * @param default_value Default Value
           */
      81   Costmap2D(  
           unsigned int cells_size_x,   unsigned int cells_size_y,   double resolution,  
           double origin_x,   double origin_y,   unsigned char default_value = 0 );
          
           /**
           * @brief Copy constructor for a costmap,   creates a copy efficiently
           * @param map The costmap to copy
           */
      89   Costmap2D(  const Costmap2D & map );
          
           /**
           * @brief Constructor for a costmap from an OccupancyGrid map
           * @param map The OccupancyGrid map to create costmap from
           */
      95   explicit Costmap2D(  const nav_msgs::msg::OccupancyGrid & map );
          
           /**
           * @brief Overloaded assignment operator
           * @param map The costmap to copy
           * @return A reference to the map after the copy has finished
           */
     102   Costmap2D & operator=(  const Costmap2D & map );
          
           /**
           * @brief Turn this costmap into a copy of a window of a costmap passed in
           * @param map The costmap to copy
           * @param win_origin_x The x origin (  lower left corner ) for the window to copy,   in meters
           * @param win_origin_y The y origin (  lower left corner ) for the window to copy,   in meters
           * @param win_size_x The x size of the window,   in meters
           * @param win_size_y The y size of the window,   in meters
           */
     112   bool copyCostmapWindow(  
           const Costmap2D & map,   double win_origin_x,   double win_origin_y,  
           double win_size_x,  
           double win_size_y );
          
           /**
           * @brief Copies the (  x0,  y0 )..(  xn,  yn ) window from source costmap into a current costmap
           @param source Source costmap where the window will be copied from
           @param sx0 Lower x-boundary of the source window to copy,   in cells
           @param sy0 Lower y-boundary of the source window to copy,   in cells
           @param sxn Upper x-boundary of the source window to copy,   in cells
           @param syn Upper y-boundary of the source window to copy,   in cells
           @param dx0 Lower x-boundary of the destination window to copy,   in cells
           @param dx0 Lower y-boundary of the destination window to copy,   in cells
           @returns true if copy was succeeded or false in negative case
           */
     128   bool copyWindow(  
           const Costmap2D & source,  
           unsigned int sx0,   unsigned int sy0,   unsigned int sxn,   unsigned int syn,  
           unsigned int dx0,   unsigned int dy0 );
          
           /**
           * @brief Default constructor
           */
     136   Costmap2D(   );
          
           /**
           * @brief Destructor
           */
     141   virtual ~Costmap2D(   );
          
           /**
           * @brief Get the cost of a cell in the costmap
           * @param mx The x coordinate of the cell
           * @param my The y coordinate of the cell
           * @return The cost of the cell
           */
     149   unsigned char getCost(  unsigned int mx,   unsigned int my ) const;
          
           /**
           * @brief Get the cost of a cell in the costmap
           * @param mx The x coordinate of the cell
           * @param my The y coordinate of the cell
           * @return The cost of the cell
           */
     157   unsigned char getCost(  unsigned int index ) const;
          
           /**
           * @brief Set the cost of a cell in the costmap
           * @param mx The x coordinate of the cell
           * @param my The y coordinate of the cell
           * @param cost The cost to set the cell to
           */
     165   void setCost(  unsigned int mx,   unsigned int my,   unsigned char cost );
          
           /**
           * @brief Convert from map coordinates to world coordinates
           * @param mx The x map coordinate
           * @param my The y map coordinate
           * @param wx Will be set to the associated world x coordinate
           * @param wy Will be set to the associated world y coordinate
           */
     174   void mapToWorld(  unsigned int mx,   unsigned int my,   double & wx,   double & wy ) const;
          
           /**
           * @brief Convert from world coordinates to map coordinates
           * @param wx The x world coordinate
           * @param wy The y world coordinate
           * @param mx Will be set to the associated map x coordinate
           * @param my Will be set to the associated map y coordinate
           * @return True if the conversion was successful (  legal bounds ) false otherwise
           */
     184   bool worldToMap(  double wx,   double wy,   unsigned int & mx,   unsigned int & my ) const;
          
           /**
           * @brief Convert from world coordinates to map coordinates without checking for legal bounds
           * @param wx The x world coordinate
           * @param wy The y world coordinate
           * @param mx Will be set to the associated map x coordinate
           * @param my Will be set to the associated map y coordinate
           * @note The returned map coordinates <b>are not guaranteed to lie within the map.</b>
           */
     194   void worldToMapNoBounds(  double wx,   double wy,   int & mx,   int & my ) const;
          
           /**
           * @brief Convert from world coordinates to map coordinates,   constraining results to legal bounds.
           * @param wx The x world coordinate
           * @param wy The y world coordinate
           * @param mx Will be set to the associated map x coordinate
           * @param my Will be set to the associated map y coordinate
           * @note The returned map coordinates are guaranteed to lie within the map.
           */
     204   void worldToMapEnforceBounds(  double wx,   double wy,   int & mx,   int & my ) const;
          
           /**
           * @brief Given two map coordinates... compute the associated index
           * @param mx The x coordinate
           * @param my The y coordinate
           * @return The associated index
           */
     212   inline unsigned int getIndex(  unsigned int mx,   unsigned int my ) const
           {
           return my * size_x_ + mx;
           }
          
           /**
           * @brief Given an index... compute the associated map coordinates
           * @param index The index
           * @param mx Will be set to the x coordinate
           * @param my Will be set to the y coordinate
           */
     223   inline void indexToCells(  unsigned int index,   unsigned int & mx,   unsigned int & my ) const
           {
           my = index / size_x_;
           mx = index - (  my * size_x_ );
           }
          
           /**
           * @brief Will return a pointer to the underlying unsigned char array used as the costmap
           * @return A pointer to the underlying unsigned char array storing cost values
           */
     233   unsigned char * getCharMap(   ) const;
          
           /**
           * @brief Accessor for the x size of the costmap in cells
           * @return The x size of the costmap
           */
     239   unsigned int getSizeInCellsX(   ) const;
          
           /**
           * @brief Accessor for the y size of the costmap in cells
           * @return The y size of the costmap
           */
     245   unsigned int getSizeInCellsY(   ) const;
          
           /**
           * @brief Accessor for the x size of the costmap in meters
           * @return The x size of the costmap (  returns the centerpoint of the last legal cell in the map )
           */
     251   double getSizeInMetersX(   ) const;
          
           /**
           * @brief Accessor for the y size of the costmap in meters
           * @return The y size of the costmap (  returns the centerpoint of the last legal cell in the map )
           */
     257   double getSizeInMetersY(   ) const;
          
           /**
           * @brief Accessor for the x origin of the costmap
           * @return The x origin of the costmap
           */
     263   double getOriginX(   ) const;
          
           /**
           * @brief Accessor for the y origin of the costmap
           * @return The y origin of the costmap
           */
     269   double getOriginY(   ) const;
          
           /**
           * @brief Accessor for the resolution of the costmap
           * @return The resolution of the costmap
           */
     275   double getResolution(   ) const;
          
           /**
           * @brief Set the default background value of the costmap
           * @param c default value
           */
     281   void setDefaultValue(  unsigned char c )
           {
           default_value_ = c;
           }
          
           /**
           * @brief Get the default background value of the costmap
           * @return default value
           */
     290   unsigned char getDefaultValue(   )
           {
           return default_value_;
           }
          
           /**
           * @brief Sets the cost of a convex polygon to a desired value
           * @param polygon The polygon to perform the operation on
           * @param cost_value The value to set costs to
           * @return True if the polygon was filled... false if it could not be filled
           */
     301   bool setConvexPolygonCost(  
     302   const std::vector<geometry_msgs::msg::Point> & polygon,  
           unsigned char cost_value );
          
           /**
           * @brief Get the map cells that make up the outline of a polygon
           * @param polygon The polygon in map coordinates to rasterize
           * @param polygon_cells Will be set to the cells contained in the outline of the polygon
           */
     310   void polygonOutlineCells(  
     311   const std::vector<MapLocation> & polygon,  
     312   std::vector<MapLocation> & polygon_cells );
          
           /**
           * @brief Get the map cells that fill a convex polygon
           * @param polygon The polygon in map coordinates to rasterize
           * @param polygon_cells Will be set to the cells that fill the polygon
           */
     319   void convexFillCells(  
     320   const std::vector<MapLocation> & polygon,  
     321   std::vector<MapLocation> & polygon_cells );
          
           /**
           * @brief Move the origin of the costmap to a new location.... keeping data when it can
           * @param new_origin_x The x coordinate of the new origin
           * @param new_origin_y The y coordinate of the new origin
           */
     328   virtual void updateOrigin(  double new_origin_x,   double new_origin_y );
          
           /**
           * @brief Save the costmap out to a pgm file
           * @param file_name The name of the file to save
           */
     334   bool saveMap(  std::string file_name );
          
           /**
           * @brief Resize the costmap
           */
     339   void resizeMap(  
           unsigned int size_x,   unsigned int size_y,   double resolution,   double origin_x,  
           double origin_y );
          
           /**
           * @brief Reset the costmap in bounds
           */
     346   void resetMap(  unsigned int x0,   unsigned int y0,   unsigned int xn,   unsigned int yn );
          
           /**
           * @brief Reset the costmap in bounds to a value
           */
     351   void resetMapToValue(  
           unsigned int x0,   unsigned int y0,   unsigned int xn,   unsigned int yn,   unsigned char value );
          
           /**
           * @brief Given distance in the world... convert it to cells
           * @param world_dist The world distance
           * @return The equivalent cell distance
           */
     359   unsigned int cellDistance(  double world_dist );
          
           // Provide a typedef to ease future code maintenance
           typedef std::recursive_mutex mutex_t;
     363   mutex_t * getMutex(   )
           {
           return access_;
           }
          
          protected:
           /**
           * @brief Copy a region of a source map into a destination map
           * @param source_map The source map
           * @param sm_lower_left_x The lower left x point of the source map to start the copy
           * @param sm_lower_left_y The lower left y point of the source map to start the copy
           * @param sm_size_x The x size of the source map
           * @param dest_map The destination map
           * @param dm_lower_left_x The lower left x point of the destination map to start the copy
           * @param dm_lower_left_y The lower left y point of the destination map to start the copy
           * @param dm_size_x The x size of the destination map
           * @param region_size_x The x size of the region to copy
           * @param region_size_y The y size of the region to copy
           */
           template<typename data_type>
           void copyMapRegion(  
           data_type * source_map,   unsigned int sm_lower_left_x,  
           unsigned int sm_lower_left_y,  
           unsigned int sm_size_x,   data_type * dest_map,   unsigned int dm_lower_left_x,  
           unsigned int dm_lower_left_y,   unsigned int dm_size_x,   unsigned int region_size_x,  
           unsigned int region_size_y )
           {
           // we'll first need to compute the starting points for each map
           data_type * sm_index = source_map + (  sm_lower_left_y * sm_size_x + sm_lower_left_x );
           data_type * dm_index = dest_map + (  dm_lower_left_y * dm_size_x + dm_lower_left_x );
          
           // now,   we'll copy the source map into the destination map
           for (  unsigned int i = 0; i < region_size_y; ++i ) {
           memcpy(  dm_index,   sm_index,   region_size_x * sizeof(  data_type ) );
           sm_index += sm_size_x;
           dm_index += dm_size_x;
           }
           }
          
           /**
           * @brief Deletes the costmap,   static_map,   and markers data structures
           */
           virtual void deleteMaps(   );
          
           /**
           * @brief Resets the costmap and static_map to be unknown space
           */
     410   virtual void resetMaps(   );
          
           /**
           * @brief Initializes the costmap,   static_map,   and markers data structures
           * @param size_x The x size to use for map initialization
           * @param size_y The y size to use for map initialization
           */
           virtual void initMaps(  unsigned int size_x,   unsigned int size_y );
          
           /**
           * @brief Raytrace a line and apply some action at each step
           * @param at The action to take... a functor
           * @param x0 The starting x coordinate
           * @param y0 The starting y coordinate
           * @param x1 The ending x coordinate
           * @param y1 The ending y coordinate
           * @param max_length The maximum desired length of the segment...
           * allows you to not go all the way to the endpoint
           * @param min_length The minimum desired length of the segment
           */
           template<class ActionType>
           inline void raytraceLine(  
           ActionType at,   unsigned int x0,   unsigned int y0,   unsigned int x1,  
           unsigned int y1,  
           unsigned int max_length = UINT_MAX,   unsigned int min_length = 0 )
           {
           int dx_full = x1 - x0;
           int dy_full = y1 - y0;
          
           // we need to chose how much to scale our dominant dimension,  
           // based on the maximum length of the line
           double dist = std::hypot(  dx_full,   dy_full );
           if (  dist < min_length ) {
           return;
           }
          
           unsigned int min_x0,   min_y0;
           if (  dist > 0.0 ) {
           // Adjust starting point and offset to start from min_length distance
           min_x0 = (  unsigned int )(  x0 + dx_full / dist * min_length );
           min_y0 = (  unsigned int )(  y0 + dy_full / dist * min_length );
           } else {
           // dist can be 0 if [x0,   y0]==[x1,   y1].
           // In this case only this cell should be processed.
           min_x0 = x0;
           min_y0 = y0;
           }
           unsigned int offset = min_y0 * size_x_ + min_x0;
          
           int dx = x1 - min_x0;
           int dy = y1 - min_y0;
          
           unsigned int abs_dx = abs(  dx );
           unsigned int abs_dy = abs(  dy );
          
           int offset_dx = sign(  dx );
           int offset_dy = sign(  dy ) * size_x_;
          
           double scale = (  dist == 0.0 ) ? 1.0 : std::min(  1.0,   max_length / dist );
           // if x is dominant
           if (  abs_dx >= abs_dy ) {
           int error_y = abs_dx / 2;
          
           bresenham2D(  
           at,   abs_dx,   abs_dy,   error_y,   offset_dx,   offset_dy,   offset,   (  unsigned int )(  scale * abs_dx ) );
           return;
           }
          
           // otherwise y is dominant
           int error_x = abs_dy / 2;
          
           bresenham2D(  
           at,   abs_dy,   abs_dx,   error_x,   offset_dy,   offset_dx,   offset,   (  unsigned int )(  scale * abs_dy ) );
           }
          
          private:
           /**
           * @brief A 2D implementation of Bresenham's raytracing algorithm...
           * applies an action at each step
           */
           template<class ActionType>
           inline void bresenham2D(  
           ActionType at,   unsigned int abs_da,   unsigned int abs_db,   int error_b,  
           int offset_a,  
           int offset_b,   unsigned int offset,  
           unsigned int max_length )
           {
           unsigned int end = std::min(  max_length,   abs_da );
           for (  unsigned int i = 0; i < end; ++i ) {
           at(  offset );
           offset += offset_a;
           error_b += abs_db;
           if (  (  unsigned int )error_b >= abs_da ) {
           offset += offset_b;
           error_b -= abs_da;
           }
           }
           at(  offset );
           }
          
           /**
           * @brief get the sign of an int
           */
           inline int sign(  int x )
           {
           return x > 0 ? 1.0 : -1.0;
           }
          
           mutex_t * access_;
          
          protected:
           unsigned int size_x_;
           unsigned int size_y_;
           double resolution_;
           double origin_x_;
           double origin_y_;
           unsigned char * costmap_;
           unsigned char default_value_;
          
           // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
           class MarkCell
           {
           public:
           MarkCell(  unsigned char * costmap,   unsigned char value )
           : costmap_(  costmap ),   value_(  value )
           {
           }
           inline void operator(   )(  unsigned int offset )
           {
           costmap_[offset] = value_;
           }
          
           private:
           unsigned char * costmap_;
           unsigned char value_;
           };
          
           class PolygonOutlineCells
           {
           public:
           PolygonOutlineCells(  
           const Costmap2D & costmap,   const unsigned char * /*char_map*/,  
           std::vector<MapLocation> & cells )
           : costmap_(  costmap ),   cells_(  cells )
           {
           }
          
           // just push the relevant cells back onto the list
           inline void operator(   )(  unsigned int offset )
           {
           MapLocation loc;
           costmap_.indexToCells(  offset,   loc.x,   loc.y );
           cells_.push_back(  loc );
           }
          
           private:
           const Costmap2D & costmap_;
           std::vector<MapLocation> & cells_;
           };
           // *INDENT-ON*
          };
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_2D_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * Copyright (  c ) 2019,   Samsung Research America,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
          
          #include <algorithm>
          #include <string>
          #include <memory>
          
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "map_msgs/msg/occupancy_grid_update.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_msgs/srv/get_costmap.hpp"
          #include "tf2/transform_datatypes.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "tf2/LinearMath/Quaternion.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class Costmap2DPublisher
           * @brief A tool to periodically publish visualization data from a Costmap2D
           */
      63  class Costmap2DPublisher
          {
          public:
           /**
           * @brief Constructor for the Costmap2DPublisher
           */
      69   Costmap2DPublisher(  
      70   const nav2_util::LifecycleNode::WeakPtr & parent,  
      71   Costmap2D * costmap,  
      72   std::string global_frame,  
      73   std::string topic_name,  
      74   bool always_send_full_costmap = false );
          
           /**
           * @brief Destructor
           */
      79   ~Costmap2DPublisher(   );
          
           /**
           * @brief Configure node
           */
      84   void on_configure(   ) {}
          
           /**
           * @brief Activate node
           */
      89   void on_activate(   )
           {
           costmap_pub_->on_activate(   );
           costmap_update_pub_->on_activate(   );
           costmap_raw_pub_->on_activate(   );
           }
          
           /**
           * @brief deactivate node
           */
      99   void on_deactivate(   )
           {
           costmap_pub_->on_deactivate(   );
           costmap_update_pub_->on_deactivate(   );
           costmap_raw_pub_->on_deactivate(   );
           }
          
           /**
           * @brief Cleanup node
           */
     109   void on_cleanup(   ) {}
          
           /** @brief Include the given bounds in the changed-rectangle. */
     112   void updateBounds(  unsigned int x0,   unsigned int xn,   unsigned int y0,   unsigned int yn )
           {
           x0_ = std::min(  x0,   x0_ );
           xn_ = std::max(  xn,   xn_ );
           y0_ = std::min(  y0,   y0_ );
           yn_ = std::max(  yn,   yn_ );
           }
          
           /**
           * @brief Publishes the visualization data over ROS
           */
     123   void publishCostmap(   );
          
           /**
           * @brief Check if the publisher is active
           * @return True if the frequency for the publisher is non-zero,   false otherwise
           */
     129   bool active(   )
           {
           return active_;
           }
          
          private:
           /** @brief Prepare grid_ message for publication. */
     136   void prepareGrid(   );
     137   void prepareCostmap(   );
          
           /** @brief Publish the latest full costmap to the new subscriber. */
           // void onNewSubscription(  const ros::SingleSubscriberPublisher& pub );
          
           /** @brief GetCostmap callback service */
     143   void costmap_service_callback(  
     144   const std::shared_ptr<rmw_request_id_t> request_header,  
     145   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,  
     146   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response );
          
     148   rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_costmap_2d" )};
          
           Costmap2D * costmap_;
           std::string global_frame_;
           std::string topic_name_;
           unsigned int x0_,   xn_,   y0_,   yn_;
           double saved_origin_x_;
           double saved_origin_y_;
           bool active_;
           bool always_send_full_costmap_;
          
           // Publisher for translated costmap values as msg::OccupancyGrid used in visualization
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
           rclcpp_lifecycle::LifecyclePublisher<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
           costmap_update_pub_;
          
           // Publisher for raw costmap values as msg::Costmap from layered costmap
           rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
          
           // Service for getting the costmaps
           rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
          
           float grid_resolution;
           unsigned int grid_width,   grid_height;
           std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_;
           std::unique_ptr<nav2_msgs::msg::Costmap> costmap_raw_;
           // Translate from 0-255 values in costmap to -1 to 100 values in message.
           static char * cost_translation_table_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/polygon.h"
          #include "geometry_msgs/msg/polygon_stamped.h"
          #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          #include "nav2_costmap_2d/clear_costmap_service.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/layer.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "tf2/convert.h"
          #include "tf2/LinearMath/Transform.h"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2/time.h"
          #include "tf2/transform_datatypes.h"
          
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          namespace nav2_costmap_2d
          {
          
          /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
           * topics that provide observations about obstacles in either the form
           * of PointCloud or LaserScan messages. */
      72  class Costmap2DROS : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief Constructor for the wrapper,   the node will
           * be placed in a namespace equal to the node's name
           * @param name Name of the costmap ROS node
           */
      80   explicit Costmap2DROS(  const std::string & name );
          
           /**
           * @brief Constructor for the wrapper
           * @param name Name of the costmap ROS node
           * @param parent_namespace Absolute namespace of the node hosting the costmap node
           * @param local_namespace Namespace to append to the parent namespace
           */
      88   explicit Costmap2DROS(  
      89   const std::string & name,  
      90   const std::string & parent_namespace,  
      91   const std::string & local_namespace );
          
           /**
           * @brief A destructor
           */
      96   ~Costmap2DROS(   );
          
           /**
           * @brief Configure node
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Activate node
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Deactivate node
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Cleanup node
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief shutdown node
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Subscribes to sensor topics if necessary and starts costmap
           * updates,   can be called to restart the costmap after calls to either
           * stop(   ) or pause(   )
           */
           void start(   );
          
           /**
           * @brief Stops costmap updates and unsubscribes from sensor topics
           */
           void stop(   );
          
           /**
           * @brief Stops the costmap from updating,   but sensor data still comes in over the wire
           */
           void pause(   );
          
           /**
           * @brief Resumes costmap updates
           */
           void resume(   );
          
           /**
           * @brief Update the map with the layered costmap / plugins
           */
           void updateMap(   );
          
           /**
           * @brief Reset each individual layer
           */
           void resetLayers(   );
          
           /** @brief Same as getLayeredCostmap(   )->isCurrent(   ). */
           bool isCurrent(   )
           {
           return layered_costmap_->isCurrent(   );
           }
          
           /**
           * @brief Get the pose of the robot in the global frame of the costmap
           * @param global_pose Will be set to the pose of the robot in the global frame of the costmap
           * @return True if the pose was set successfully,   false otherwise
           */
           bool getRobotPose(  geometry_msgs::msg::PoseStamped & global_pose );
          
           /**
           * @brief Transform the input_pose in the global frame of the costmap
           * @param input_pose pose to be transformed
           * @param transformed_pose pose transformed
           * @return True if the pose was transformed successfully,   false otherwise
           */
     174   bool transformPoseToGlobalFrame(  
           const geometry_msgs::msg::PoseStamped & input_pose,  
           geometry_msgs::msg::PoseStamped & transformed_pose );
          
           /** @brief Returns costmap name */
     179   std::string getName(   ) const
           {
           return name_;
           }
          
           /** @brief Returns the delay in transform (  tf ) data that is tolerable in seconds */
     185   double getTransformTolerance(   ) const
           {
           return transform_tolerance_;
           }
          
           /**
           * @brief Return a pointer to the "master" costmap which receives updates from all the layers.
           *
           * Same as calling getLayeredCostmap(   )->getCostmap(   ).
           */
     195   Costmap2D * getCostmap(   )
           {
           return layered_costmap_->getCostmap(   );
           }
          
           /**
           * @brief Returns the global frame of the costmap
           * @return The global frame of the costmap
           */
     204   std::string getGlobalFrameID(   )
           {
           return global_frame_;
           }
          
           /**
           * @brief Returns the local frame of the costmap
           * @return The local frame of the costmap
           */
     213   std::string getBaseFrameID(   )
           {
           return robot_base_frame_;
           }
          
           /**
           * @brief Get the layered costmap object used in the node
           */
     221   LayeredCostmap * getLayeredCostmap(   )
           {
           return layered_costmap_.get(   );
           }
          
           /** @brief Returns the current padded footprint as a geometry_msgs::msg::Polygon. */
     227   geometry_msgs::msg::Polygon getRobotFootprintPolygon(   )
           {
           return nav2_costmap_2d::toPolygon(  padded_footprint_ );
           }
          
           /** @brief Return the current footprint of the robot as a vector of points.
           *
           * This version of the footprint is padded by the footprint_padding_
           * distance,   set in the rosparam "footprint_padding".
           *
           * The footprint initially comes from the rosparam "footprint" but
           * can be overwritten by dynamic reconfigure or by messages received
           * on the "footprint" topic. */
     240   std::vector<geometry_msgs::msg::Point> getRobotFootprint(   )
           {
           return padded_footprint_;
           }
          
           /** @brief Return the current unpadded footprint of the robot as a vector of points.
           *
           * This is the raw version of the footprint without padding.
           *
           * The footprint initially comes from the rosparam "footprint" but
           * can be overwritten by dynamic reconfigure or by messages received
           * on the "footprint" topic. */
     252   std::vector<geometry_msgs::msg::Point> getUnpaddedRobotFootprint(   )
           {
           return unpadded_footprint_;
           }
          
           /**
           * @brief Build the oriented footprint of the robot at the robot's current pose
           * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
           */
     261   void getOrientedFootprint(  std::vector<geometry_msgs::msg::Point> & oriented_footprint );
          
           /** @brief Set the footprint of the robot to be the given set of
           * points,   padded by footprint_padding.
           *
           * Should be a convex polygon,   though this is not enforced.
           *
           * First expands the given polygon by footprint_padding_ and then
           * sets padded_footprint_ and calls
           * layered_costmap_->setFootprint(   ). Also saves the unpadded
           * footprint,   which is available from
           * getUnpaddedRobotFootprint(   ). */
     273   void setRobotFootprint(  const std::vector<geometry_msgs::msg::Point> & points );
          
           /** @brief Set the footprint of the robot to be the given polygon,  
           * padded by footprint_padding.
           *
           * Should be a convex polygon,   though this is not enforced.
           *
           * First expands the given polygon by footprint_padding_ and then
           * sets padded_footprint_ and calls
           * layered_costmap_->setFootprint(   ). Also saves the unpadded
           * footprint,   which is available from
           * getUnpaddedRobotFootprint(   ). */
     285   void setRobotFootprintPolygon(  const geometry_msgs::msg::Polygon::SharedPtr footprint );
          
     287   std::shared_ptr<tf2_ros::Buffer> getTfBuffer(   ) {return tf_buffer_;}
          
           /**
           * @brief Get the costmap's use_radius_ parameter,   corresponding to
           * whether the footprint for the robot is a circle with radius robot_radius_
           * or an arbitrarily defined footprint in footprint_.
           * @return use_radius_
           */
     295   bool getUseRadius(   ) {return use_radius_;}
          
           /**
           * @brief Get the costmap's robot_radius_ parameter,   corresponding to
           * raidus of the robot footprint when it is defined as as circle
           * (  i.e. when use_radius_ == true ).
           * @return robot_radius_
           */
     303   double getRobotRadius(   ) {return robot_radius_;}
          
          protected:
           // Publishers and subscribers
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
           footprint_pub_;
     309   std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
          
           rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
           rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;
          
           // Dedicated callback group and executor for tf timer_interface and message fillter
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
           std::unique_ptr<nav2_util::NodeThread> executor_thread_;
          
           // Transform listener
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          
           std::unique_ptr<LayeredCostmap> layered_costmap_{nullptr};
           std::string name_;
           std::string parent_namespace_;
          
           /**
           * @brief Function on timer for costmap update
           */
           void mapUpdateLoop(  double frequency );
           bool map_update_thread_shutdown_{false};
           bool stop_updates_{false};
           bool initialized_{false};
           bool stopped_{true};
           std::unique_ptr<std::thread> map_update_thread_; ///< @brief A thread for updating the map
           rclcpp::Time last_publish_{0,   0,   RCL_ROS_TIME};
           rclcpp::Duration publish_cycle_{1,   0};
           pluginlib::ClassLoader<Layer> plugin_loader_{"nav2_costmap_2d",   "nav2_costmap_2d::Layer"};
          
           /**
           * @brief Get parameters for node
           */
           void getParameters(   );
           bool always_send_full_costmap_{false};
           std::string footprint_;
           float footprint_padding_{0};
           std::string global_frame_; ///< The global frame for the costmap
           int map_height_meters_{0};
           double map_publish_frequency_{0};
           double map_update_frequency_{0};
           int map_width_meters_{0};
           double origin_x_{0};
           double origin_y_{0};
           std::vector<std::string> default_plugins_;
           std::vector<std::string> default_types_;
           std::vector<std::string> plugin_names_;
           std::vector<std::string> plugin_types_;
           std::vector<std::string> filter_names_;
           std::vector<std::string> filter_types_;
           double resolution_{0};
           std::string robot_base_frame_; ///< The frame_id of the robot base
           double robot_radius_;
           bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap
           bool track_unknown_space_{false};
           double transform_tolerance_{0}; ///< The timeout before transform errors
          
           // Derived parameters
           bool use_radius_{false};
           std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
           std::vector<geometry_msgs::msg::Point> padded_footprint_;
          
           std::unique_ptr<ClearCostmapService> clear_costmap_service_;
          
           // Dynamic parameters handler
           OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
          
          #include <string>
          #include <mutex>
          
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "nav2_costmap_2d/layer.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid
           * hidden problems when the shared handling of costmap_ resource (  PR #1936 )
           */
      54  class CostmapFilter : public Layer
          {
          public:
           /**
           * @brief A constructor
           */
      60   CostmapFilter(   );
           /**
           * @brief A destructor
           */
      64   ~CostmapFilter(   );
          
           /**
           * @brief: Provide a typedef to ease future code maintenance
           */
           typedef std::recursive_mutex mutex_t;
           /**
           * @brief: returns pointer to a mutex
           */
      73   mutex_t * getMutex(   )
           {
           return access_;
           }
          
           /**
           * @brief Initialization process of layer on startup
           */
           void onInitialize(   ) final;
          
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,  
           double * min_x,   double * min_y,   double * max_x,   double * max_y ) final;
          
           /**
           * @brief Update the costs in the master costmap in the window
           * @param master_grid The master costmap grid to update
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           void updateCosts(  
           nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j ) final;
          
           /**
           * @brief Activate the layer
           */
           void activate(   ) final;
           /**
           * @brief Deactivate the layer
           */
           void deactivate(   ) final;
           /**
           * @brief Reset the layer
           */
           void reset(   ) final;
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
           bool isClearable(   ) {return false;}
          
           /** CostmapFilter API **/
           /**
           * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics
           * @param: Name of costmap filter info topic
           */
           virtual void initializeFilter(  
           const std::string & filter_info_topic ) = 0;
          
           /**
           * @brief: An algorithm for how to use that map's information. Fills the Costmap2D with
           * calculated data and makes an action based on processed data
           * @param: Reference to a master costmap2d
           * @param: Low window map boundary OX
           * @param: Low window map boundary OY
           * @param: High window map boundary OX
           * @param: High window map boundary OY
           * @param: Robot 2D-pose
           */
     145   virtual void process(  
           nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j,  
           const geometry_msgs::msg::Pose2D & pose ) = 0;
          
           /**
           * @brief: Resets costmap filter. Stops all subscriptions
           */
           virtual void resetFilter(   ) = 0;
          
          protected:
           /**
           * @brief: Name of costmap filter info topic
           */
           std::string filter_info_topic_;
          
           /**
           * @brief: Name of filter mask topic
           */
           std::string mask_topic_;
          
           /**
           * @brief: mask_frame_->global_frame_ transform tolerance
           */
           tf2::Duration transform_tolerance_;
          
          private:
           /**
           * @brief: Latest robot position
           */
           geometry_msgs::msg::Pose2D latest_pose_;
          
           /**
           * @brief: Mutex for locking filter's resources
           */
           mutex_t * access_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_
          
          /** Provides constants used in costmap filters */
          
          namespace nav2_costmap_2d
          {
          
          /** Types of costmap filter */
          static constexpr uint8_t KEEPOUT_FILTER = 0;
          static constexpr uint8_t SPEED_FILTER_PERCENT = 1;
          static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2;
          
          /** Default values for base and multiplier */
      52  static constexpr double BASE_DEFAULT = 0.0;
      53  static constexpr double MULTIPLIER_DEFAULT = 1.0;
          
          /** Speed filter constants */
          static constexpr int8_t SPEED_MASK_UNKNOWN = -1;
          static constexpr int8_t SPEED_MASK_NO_LIMIT = 0;
      58  static constexpr double NO_SPEED_LIMIT = 0.0;
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_
          
          #include <string>
          #include <memory>
          
          #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_msgs/msg/costmap_filter_info.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class KeepoutFilter
           * @brief Reads in a keepout mask and marks keepout regions in the map
           * to prevent planning or control in restricted areas
           */
      58  class KeepoutFilter : public CostmapFilter
          {
          public:
           /**
           * @brief A constructor
           */
      64   KeepoutFilter(   );
          
           /**
           * @brief Initialize the filter and subscribe to the info topic
           */
      69   void initializeFilter(  
      70   const std::string & filter_info_topic );
          
           /**
           * @brief Process the keepout layer at the current pose / bounds / grid
           */
      75   void process(  
      76   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j,  
      78   const geometry_msgs::msg::Pose2D & pose );
          
           /**
           * @brief Reset the costmap filter / topic / info
           */
      83   void resetFilter(   );
          
           /**
           * @brief If this filter is active
           */
      88   bool isActive(   );
          
          private:
           /**
           * @brief Callback for the filter information
           */
      94   void filterInfoCallback(  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg );
           /**
           * @brief Callback for the filter mask
           */
      98   void maskCallback(  const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
          
     100   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
     101   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
          
     103   std::unique_ptr<Costmap2D> mask_costmap_;
          
     105   std::string mask_frame_; // Frame where mask located in
     106   std::string global_frame_; // Frame of currnet layer (  master_grid )
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
          
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_msgs/msg/costmap_filter_info.hpp"
          #include "nav2_msgs/msg/speed_limit.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class SpeedFilter
           * @brief Reads in a speed restriction mask and enables a robot to
           * dynamically adjust speed based on pose in map to slow in dangerous
           * areas. Done via absolute speed setting or percentage of maximum speed
           */
      58  class SpeedFilter : public CostmapFilter
          {
          public:
           /**
           * @brief A constructor
           */
      64   SpeedFilter(   );
          
           /**
           * @brief Initialize the filter and subscribe to the info topic
           */
      69   void initializeFilter(  
      70   const std::string & filter_info_topic );
          
           /**
           * @brief Process the keepout layer at the current pose / bounds / grid
           */
      75   void process(  
      76   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j,  
      78   const geometry_msgs::msg::Pose2D & pose );
          
           /**
           * @brief Reset the costmap filter / topic / info
           */
      83   void resetFilter(   );
          
           /**
           * @brief If this filter is active
           */
      88   bool isActive(   );
          
          protected:
           /**
           * @brief: Transforms robot pose from current layer frame to mask frame
           * @param: pose Robot pose in costmap frame
           * @param: mask_pose Robot pose in mask frame
           * @return: True if the transformation was successful,   false otherwise
           */
      97   bool transformPose(  
      98   const geometry_msgs::msg::Pose2D & pose,  
      99   geometry_msgs::msg::Pose2D & mask_pose ) const;
          
           /**
           * @brief: Convert from world coordinates to mask coordinates.
           Similar to Costmap2D::worldToMap(   ) method but works directly with OccupancyGrid-s.
           * @param wx The x world coordinate
           * @param wy The y world coordinate
           * @param mx Will be set to the associated mask x coordinate
           * @param my Will be set to the associated mask y coordinate
           * @return True if the conversion was successful (  legal bounds ) false otherwise
           */
     110   bool worldToMask(  double wx,   double wy,   unsigned int & mx,   unsigned int & my ) const;
          
           /**
           * @brief Get the data of a cell in the filter mask
           * @param mx The x coordinate of the cell
           * @param my The y coordinate of the cell
           * @return The data of the selected cell
           */
     118   inline int8_t getMaskData(  
           const unsigned int mx,   const unsigned int my ) const;
          
          private:
           /**
           * @brief Callback for the filter information
           */
     125   void filterInfoCallback(  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg );
           /**
           * @brief Callback for the filter mask
           */
     129   void maskCallback(  const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
          
     131   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
     132   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
          
     134   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_pub_;
          
     136   nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;
          
     138   std::string mask_frame_; // Frame where mask located in
     139   std::string global_frame_; // Frame of currnet layer (  master_grid )
          
           double base_,   multiplier_;
     142   bool percentage_;
           double speed_limit_,   speed_limit_prev_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
          
          #include <rclcpp/rclcpp.hpp>
          #include <nav2_costmap_2d/layer.hpp>
          #include <nav2_costmap_2d/layered_costmap.hpp>
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class CostmapLayer
           * @brief A costmap layer base class for costmap plugin layers.
           * Rather than just a layer,   this object also contains an internal
           * costmap object to populate and maintain state.
           */
      54  class CostmapLayer : public Layer,   public Costmap2D
          {
          public:
           /**
           * @brief CostmapLayer constructor
           */
           CostmapLayer(   )
           : has_extra_bounds_(  false ),  
           extra_min_x_(  1e6 ),   extra_max_x_(  -1e6 ),  
           extra_min_y_(  1e6 ),   extra_max_y_(  -1e6 ) {}
          
           /**
           * @brief If layer is discrete
           */
           bool isDiscretized(   )
           {
           return true;
           }
          
           /**
           * @brief Match the size of the master costmap
           */
      76   virtual void matchSize(   );
          
           /**
           * @brief Clear an are in the costmap with the given dimension
           * if invert,   then clear everything except these dimensions
           */
           virtual void clearArea(  int start_x,   int start_y,   int end_x,   int end_y,   bool invert );
          
           /**
           * If an external source changes values in the costmap,  
           * it should call this method with the area that it changed
           * to ensure that the costmap includes this region as well.
           * @param mx0 Minimum x value of the bounding box
           * @param my0 Minimum y value of the bounding box
           * @param mx1 Maximum x value of the bounding box
           * @param my1 Maximum y value of the bounding box
           */
           void addExtraBounds(  double mx0,   double my0,   double mx1,   double my1 );
          
          protected:
           /*
           * Updates the master_grid within the specified
           * bounding box using this layer's values.
           *
           * TrueOverwrite means every value from this layer
           * is written into the master grid.
           */
           void updateWithTrueOverwrite(  
           nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j );
          
           /*
           * Updates the master_grid within the specified
           * bounding box using this layer's values.
           *
           * Overwrite means every valid value from this layer
           * is written into the master grid (  does not copy NO_INFORMATION )
           */
           void updateWithOverwrite(  
           nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j );
          
           /*
           * Updates the master_grid within the specified
           * bounding box using this layer's values.
           *
           * Sets the new value to the maximum of the master_grid's value
           * and this layer's value. If the master value is NO_INFORMATION,  
           * it is overwritten. If the layer's value is NO_INFORMATION,  
           * the master value does not change.
           */
           void updateWithMax(  
           nav2_costmap_2d::Costmap2D & master_grid,   int min_i,   int min_j,   int max_i,  
           int max_j );
          
           /*
           * Updates the master_grid within the specified
           * bounding box using this layer's values.
           *
           * Sets the new value to the sum of the master grid's value
           * and this layer's value. If the master value is NO_INFORMATION,  
           * it is overwritten with the layer's value. If the layer's value
           * is NO_INFORMATION,   then the master value does not change.
           *
           * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,  
           * the master value is set to (  INSCRIBED_INFLATED_OBSTACLE - 1 ).
           */
           void updateWithAddition(  
           nav2_costmap_2d::Costmap2D & master_grid,   int min_i,   int min_j,   int max_i,  
           int max_j );
          
           /**
           * Updates the bounding box specified in the parameters to include
           * the location (  x,  y )
           *
           * @param x x-coordinate to include
           * @param y y-coordinate to include
           * @param min_x bounding box
           * @param min_y bounding box
           * @param max_x bounding box
           * @param max_y bounding box
           */
           void touch(  double x,   double y,   double * min_x,   double * min_y,   double * max_x,   double * max_y );
          
           /*
           * Updates the bounding box specified in the parameters
           * to include the bounding box from the addExtraBounds
           * call. If addExtraBounds was not called,   the method will do nothing.
           *
           * Should be called at the beginning of the updateBounds method
           *
           * @param min_x bounding box (  input and output )
           * @param min_y bounding box (  input and output )
           * @param max_x bounding box (  input and output )
           * @param max_y bounding box (  input and output )
           */
           void useExtraBounds(  double * min_x,   double * min_y,   double * max_x,   double * max_y );
           bool has_extra_bounds_;
          
          private:
           double extra_min_x_,   extra_max_x_,   extra_min_y_,   extra_max_y_;
          };
          
          } // namespace nav2_costmap_2d
          #endif // NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_
          
          #include <math.h>
          #include <algorithm>
          #include <vector>
          
          #include "geometry_msgs/msg/point.hpp"
          
          /** @brief Return -1 if x < 0,   +1 otherwise. */
      48  inline double sign(  double x )
          {
           return x < 0.0 ? -1.0 : 1.0;
          }
          
          /** @brief Same as sign(  x ) but returns 0 if x is 0. */
      54  inline double sign0(  double x )
          {
           return x < 0.0 ? -1.0 : (  x > 0.0 ? 1.0 : 0.0 );
          }
          
          /** @brief Gets L2 norm distance */
      60  inline double distance(  double x0,   double y0,   double x1,   double y1 )
          {
           return hypot(  x1 - x0,   y1 - y0 );
          }
          
          /** @brief Gets point distance to a line */
      66  double distanceToLine(  double pX,   double pY,   double x0,   double y0,   double x1,   double y1 );
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class CostmapSubscriber
           * @brief Subscribes to the costmap via a ros topic
           */
      32  class CostmapSubscriber
          {
          public:
           /**
           * @brief A constructor
           */
      38   CostmapSubscriber(  
      39   const nav2_util::LifecycleNode::WeakPtr & parent,  
      40   const std::string & topic_name );
          
           /**
           * @brief A constructor
           */
      45   CostmapSubscriber(  
      46   const rclcpp::Node::WeakPtr & parent,  
      47   const std::string & topic_name );
          
           /**
           * @brief A destructor
           */
      52   ~CostmapSubscriber(   ) {}
          
           /**
           * @brief A Get the costmap from topic
           */
      57   std::shared_ptr<Costmap2D> getCostmap(   );
          
           /**
           * @brief Convert an occ grid message into a costmap object
           */
      62   void toCostmap2D(   );
           /**
           * @brief Callback for the costmap topic
           */
      66   void costmapCallback(  const nav2_msgs::msg::Costmap::SharedPtr msg );
          
          protected:
      69   std::shared_ptr<Costmap2D> costmap_;
      70   nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
      71   std::string topic_name_;
           bool costmap_received_{false};
           rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Modified by: Shivang Patel (  shivaan14@gmail.com )
          
          #ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
          #define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_costmap_2d/footprint_subscriber.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class CostmapTopicCollisionChecker
           * @brief Using a costmap via a ros topic,   this object is used to
           * find if robot poses are in collision with the costmap environment
           */
      40  class CostmapTopicCollisionChecker
          {
          public:
           /**
           * @brief A constructor
           */
      46   CostmapTopicCollisionChecker(  
      47   CostmapSubscriber & costmap_sub,  
      48   FootprintSubscriber & footprint_sub,  
      49   std::string name = "collision_checker" );
          
           /**
           * @brief A destructor
           */
           ~CostmapTopicCollisionChecker(   ) = default;
          
           /**
           * @brief Returns the obstacle footprint score for a particular pose
           *
           * @param pose Pose to get score at
           * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,  
           * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
           */
           double scorePose(  
           const geometry_msgs::msg::Pose2D & pose,  
           bool fetch_costmap_and_footprint = true );
          
           /**
           * @brief Returns if a pose is collision free
           *
           * @param pose Pose to check collision at
           * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,  
           * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
           */
           bool isCollisionFree(  
           const geometry_msgs::msg::Pose2D & pose,  
           bool fetch_costmap_and_footprint = true );
          
          protected:
           /**
           * @brief Get a footprint at a set pose
           *
           * @param pose Pose to get footprint at
           * @param fetch_latest_footprint Defaults to true. When checking with multiple poses at once,  
           * footprint should be fetched in the first check but fetching can be skipped in consequent checks for speedup
           */
           Footprint getFootprint(  
           const geometry_msgs::msg::Pose2D & pose,  
           bool fetch_latest_footprint = true );
          
           // Name used for logging
           std::string name_;
           CostmapSubscriber & costmap_sub_;
           FootprintSubscriber & footprint_sub_;
           FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
           rclcpp::Clock::SharedPtr clock_;
           Footprint footprint_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
          #define NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
          
          #include <stdexcept>
          #include <string>
          #include <memory>
          
          namespace nav2_costmap_2d
          {
          /**
           * @class CollisionCheckerException
           * @brief Exceptions thrown if collision checker determines a pose is in
           * collision with the environment costmap
           */
      48  class CollisionCheckerException : public std::runtime_error
          {
          public:
      51   explicit CollisionCheckerException(  const std::string description )
           : std::runtime_error(  description ) {}
          };
          
          /**
           * @class IllegalPoseException
           * @brief Thrown when CollisionChecker encounters a fatal error
           */
      59  class IllegalPoseException : public CollisionCheckerException
          {
          public:
      62   IllegalPoseException(  const std::string name,   const std::string description )
           : CollisionCheckerException(  description ),   name_(  name ) {}
      64   std::string getCriticName(   ) const {return name_;}
          
          protected:
      67   std::string name_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__EXCEPTIONS_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__FOOTPRINT_HPP_
          #define NAV2_COSTMAP_2D__FOOTPRINT_HPP_
          
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/polygon.hpp"
          #include "geometry_msgs/msg/polygon_stamped.hpp"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/point32.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @brief Calculate the extreme distances for the footprint
           *
           * @param footprint The footprint to examine
           * @param min_dist Output parameter of the minimum distance
           * @param max_dist Output parameter of the maximum distance
           */
      61  void calculateMinAndMaxDistances(  
           const std::vector<geometry_msgs::msg::Point> & footprint,  
           double & min_dist,   double & max_dist );
          
          /**
           * @brief Convert Point32 to Point
           */
      68  geometry_msgs::msg::Point toPoint(  geometry_msgs::msg::Point32 pt );
          
          /**
           * @brief Convert Point to Point32
           */
      73  geometry_msgs::msg::Point32 toPoint32(  geometry_msgs::msg::Point pt );
          
          /**
           * @brief Convert vector of Points to Polygon msg
           */
      78  geometry_msgs::msg::Polygon toPolygon(  std::vector<geometry_msgs::msg::Point> pts );
          
          /**
           * @brief Convert Polygon msg to vector of Points.
           */
      83  std::vector<geometry_msgs::msg::Point> toPointVector(  
           geometry_msgs::msg::Polygon::SharedPtr polygon );
          
          /**
           * @brief Given a pose and base footprint,   build the oriented footprint of the robot (  list of Points )
           * @param x The x position of the robot
           * @param y The y position of the robot
           * @param theta The orientation of the robot
           * @param footprint_spec Basic shape of the footprint
           * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
          */
      94  void transformFootprint(  
           double x,   double y,   double theta,  
           const std::vector<geometry_msgs::msg::Point> & footprint_spec,  
           std::vector<geometry_msgs::msg::Point> & oriented_footprint );
          
          /**
           * @brief Given a pose and base footprint,   build the oriented footprint of the robot (  PolygonStamped )
           * @param x The x position of the robot
           * @param y The y position of the robot
           * @param theta The orientation of the robot
           * @param footprint_spec Basic shape of the footprint
           * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
          */
     107  void transformFootprint(  
           double x,   double y,   double theta,  
           const std::vector<geometry_msgs::msg::Point> & footprint_spec,  
           geometry_msgs::msg::PolygonStamped & oriented_footprint );
          
          /**
           * @brief Adds the specified amount of padding to the footprint (  in place )
           */
     115  void padFootprint(  std::vector<geometry_msgs::msg::Point> & footprint,   double padding );
          
          /**
           * @brief Create a circular footprint from a given radius
           */
     120  std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(  double radius );
          
          /**
           * @brief Make the footprint from the given string.
           *
           * Format should be bracketed array of arrays of floats,   like so: [[1.0,   2.2],   [3.3,   4.2],   ...]
           *
           */
     128  bool makeFootprintFromString(  
           const std::string & footprint_string,  
           std::vector<geometry_msgs::msg::Point> & footprint );
          
          } // end namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__FOOTPRINT_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Modified by: Shivang Patel (  shivaang14@gmail.com )
          
          #ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
          #define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          namespace nav2_costmap_2d
          {
          typedef std::vector<geometry_msgs::msg::Point> Footprint;
          
          /**
           * @class FootprintCollisionChecker
           * @brief Checker for collision with a footprint on a costmap
           */
          template<typename CostmapT>
      40  class FootprintCollisionChecker
          {
          public:
           /**
           * @brief A constructor.
           */
      46   FootprintCollisionChecker(   );
           /**
           * @brief A constructor.
           */
      50   explicit FootprintCollisionChecker(  CostmapT costmap );
           /**
           * @brief Find the footprint cost in oriented footprint
           */
      54   double footprintCost(  const Footprint footprint );
           /**
           * @brief Find the footprint cost a a post with an unoriented footprint
           */
      58   double footprintCostAtPose(  double x,   double y,   double theta,   const Footprint footprint );
           /**
           * @brief Get the cost for a line segment
           */
      62   double lineCost(  int x0,   int x1,   int y0,   int y1 ) const;
           /**
           * @brief Get the map coordinates from a world point
           */
      66   bool worldToMap(  double wx,   double wy,   unsigned int & mx,   unsigned int & my );
           /**
           * @brief Get the cost of a point
           */
      70   double pointCost(  int x,   int y ) const;
           /**
           * @brief Set the current costmap object to use for collision detection
           */
      74   void setCostmap(  CostmapT costmap );
           /**
           * @brief Get the current costmap object
           */
      78   CostmapT getCostmap(   )
           {
           return costmap_;
           }
          
          protected:
      84   CostmapT costmap_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
          #define NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
          
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/robot_utils.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class FootprintSubscriber
           * @brief Subscriber to the footprint topic to get current robot footprint
           * (  if changing ) for use in collision avoidance
           */
      33  class FootprintSubscriber
          {
          public:
           /**
           * @brief A constructor
           */
      39   FootprintSubscriber(  
      40   const nav2_util::LifecycleNode::WeakPtr & parent,  
      41   const std::string & topic_name,  
      42   tf2_ros::Buffer & tf,  
      43   std::string robot_base_frame = "base_link",  
           double transform_tolerance = 0.1 );
          
           /**
           * @brief A constructor
           */
      49   FootprintSubscriber(  
      50   const rclcpp::Node::WeakPtr & parent,  
      51   const std::string & topic_name,  
      52   tf2_ros::Buffer & tf,  
      53   std::string robot_base_frame = "base_link",  
           double transform_tolerance = 0.1 );
          
           /**
           * @brief A destructor
           */
      59   ~FootprintSubscriber(   ) {}
          
           /**
           * @brief Returns the latest robot footprint,   in the form as received from topic (  oriented ).
           *
           * @param footprint Output param. Latest received footprint
           * @param footprint_header Output param. Header associated with the footprint
           * @return False if no footprint has been received
           */
      68   bool getFootprintRaw(  
      69   std::vector<geometry_msgs::msg::Point> & footprint,  
      70   std_msgs::msg::Header & footprint_header );
          
           /**
           * @brief Returns the latest robot footprint,   transformed into robot base frame (  unoriented ).
           *
           * @param footprint Output param. Latest received footprint,   unoriented
           * @param footprint_header Output param. Header associated with the footprint
           * @return False if no footprint has been received or if transformation failed
           */
      79   bool getFootprintInRobotFrame(  
      80   std::vector<geometry_msgs::msg::Point> & footprint,  
      81   std_msgs::msg::Header & footprint_header );
          
          protected:
           /**
           * @brief Callback to process new footprint updates.
           */
      87   void footprint_callback(  const geometry_msgs::msg::PolygonStamped::SharedPtr msg );
          
      89   tf2_ros::Buffer & tf_;
      90   std::string robot_base_frame_;
           double transform_tolerance_;
           bool footprint_received_{false};
           geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
           rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
          #define NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
          
          #include <map>
          #include <vector>
          #include <mutex>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/layer.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          
          namespace nav2_costmap_2d
          {
          /**
           * @class CellData
           * @brief Storage for cell information used during obstacle inflation
           */
      55  class CellData
          {
          public:
           /**
           * @brief Constructor for a CellData objects
           * @param i The index of the cell in the cost map
           * @param x The x coordinate of the cell in the cost map
           * @param y The y coordinate of the cell in the cost map
           * @param sx The x coordinate of the closest obstacle cell in the costmap
           * @param sy The y coordinate of the closest obstacle cell in the costmap
           * @return
           */
      67   CellData(  double i,   unsigned int x,   unsigned int y,   unsigned int sx,   unsigned int sy )
           : index_(  static_cast<unsigned int>(  i ) ),   x_(  x ),   y_(  y ),   src_x_(  sx ),   src_y_(  sy )
           {
           }
           unsigned int index_;
           unsigned int x_,   y_;
           unsigned int src_x_,   src_y_;
          };
          
          /**
           * @class InflationLayer
           * @brief Layer to convolve costmap by robot's radius or footprint to prevent
           * collisions and largely simply collision checking
           */
      81  class InflationLayer : public Layer
          {
          public:
           /**
           * @brief A constructor
           */
      87   InflationLayer(   );
          
           /**
           * @brief A destructor
           */
      92   ~InflationLayer(   );
          
           /**
           * @brief Initialization process of layer on startup
           */
           void onInitialize(   ) override;
          
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y ) override;
           /**
           * @brief Update the costs in the master costmap in the window
           * @param master_grid The master costmap grid to update
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           void updateCosts(  
           nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j ) override;
          
           /**
           * @brief Match the size of the master costmap
           */
           void matchSize(   ) override;
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
           virtual bool isClearable(   ) {return false;}
          
           /**
           * @brief Reset this costmap
           */
           void reset(   ) override
           {
           matchSize(   );
     142   current_ = false;
           }
          
           /** @brief Given a distance,   compute a cost.
           * @param distance The distance from an obstacle in cells
           * @return A cost value for the distance */
     148   inline unsigned char computeCost(  double distance ) const
           {
           unsigned char cost = 0;
           if (  distance == 0 ) {
           cost = LETHAL_OBSTACLE;
           } else if (  distance * resolution_ <= inscribed_radius_ ) {
           cost = INSCRIBED_INFLATED_OBSTACLE;
           } else {
           // make sure cost falls off by Euclidean distance
           double factor =
           exp(  -1.0 * cost_scaling_factor_ * (  distance * resolution_ - inscribed_radius_ ) );
           cost = static_cast<unsigned char>(  (  INSCRIBED_INFLATED_OBSTACLE - 1 ) * factor );
           }
           return cost;
           }
          
           // Provide a typedef to ease future code maintenance
           typedef std::recursive_mutex mutex_t;
          
           /**
           * @brief Get the mutex of the inflation inforamtion
           */
     170   mutex_t * getMutex(   )
           {
           return access_;
           }
          
          protected:
           /**
           * @brief Process updates on footprint changes to the inflation layer
           */
           void onFootprintChanged(   ) override;
          
          private:
           /**
           * @brief Lookup pre-computed distances
           * @param mx The x coordinate of the current cell
           * @param my The y coordinate of the current cell
           * @param src_x The x coordinate of the source cell
           * @param src_y The y coordinate of the source cell
           * @return
           */
           inline double distanceLookup(  
           unsigned int mx,   unsigned int my,   unsigned int src_x,  
           unsigned int src_y )
           {
           unsigned int dx = (  mx > src_x ) ? mx - src_x : src_x - mx;
           unsigned int dy = (  my > src_y ) ? my - src_y : src_y - my;
           return cached_distances_[dx * cache_length_ + dy];
           }
          
           /**
           * @brief Lookup pre-computed costs
           * @param mx The x coordinate of the current cell
           * @param my The y coordinate of the current cell
           * @param src_x The x coordinate of the source cell
           * @param src_y The y coordinate of the source cell
           * @return
           */
           inline unsigned char costLookup(  
           unsigned int mx,   unsigned int my,   unsigned int src_x,  
           unsigned int src_y )
           {
           unsigned int dx = (  mx > src_x ) ? mx - src_x : src_x - mx;
           unsigned int dy = (  my > src_y ) ? my - src_y : src_y - my;
           return cached_costs_[dx * cache_length_ + dy];
           }
          
           /**
           * @brief Compute cached dsitances
           */
           void computeCaches(   );
          
           /**
           * @brief Compute cached dsitances
           */
           int generateIntegerDistances(   );
          
           /**
           * @brief Compute cached dsitances
           */
           unsigned int cellDistance(  double world_dist )
           {
           return layered_costmap_->getCostmap(   )->cellDistance(  world_dist );
           }
          
           /**
           * @brief Enqueue new cells in cache distance update search
           */
           inline void enqueue(  
           unsigned int index,   unsigned int mx,   unsigned int my,  
           unsigned int src_x,   unsigned int src_y );
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           double inflation_radius_,   inscribed_radius_,   cost_scaling_factor_;
           bool inflate_unknown_,   inflate_around_unknown_;
           unsigned int cell_inflation_radius_;
           unsigned int cached_cell_inflation_radius_;
           std::vector<std::vector<CellData>> inflation_cells_;
          
           double resolution_;
          
           std::vector<bool> seen_;
          
           std::vector<unsigned char> cached_costs_;
           std::vector<double> cached_distances_;
           std::vector<std::vector<int>> distance_matrix_;
           unsigned int cache_length_;
           double last_min_x_,   last_min_y_,   last_max_x_,   last_max_y_;
          
           // Indicates that the entire costmap should be reinflated next time around.
           bool need_reinflation_;
           mutex_t * access_;
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__LAYER_HPP_
          #define NAV2_COSTMAP_2D__LAYER_HPP_
          
          #include <string>
          #include <vector>
          #include <unordered_set>
          
          #include "tf2_ros/buffer.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace nav2_costmap_2d
          {
      52  class LayeredCostmap;
          
          /**
           * @class Layer
           * @brief Abstract class for layered costmap plugin implementations
           */
      58  class Layer
          {
          public:
           /**
           * @brief A constructor
           */
      64   Layer(   );
           /**
           * @brief A destructor
           */
      68   virtual ~Layer(   ) {}
          
           /**
           * @brief Initialization process of layer on startup
           */
      73   void initialize(  
      74   LayeredCostmap * parent,  
      75   std::string name,  
      76   tf2_ros::Buffer * tf,  
      77   const nav2_util::LifecycleNode::WeakPtr & node,  
      78   rclcpp::CallbackGroup::SharedPtr callback_group );
           /** @brief Stop publishers. */
      80   virtual void deactivate(   ) {}
           /** @brief Restart publishers if they've been stopped. */
      82   virtual void activate(   ) {}
           /**
           * @brief Reset this costmap
           */
      86   virtual void reset(   ) = 0;
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
      90   virtual bool isClearable(   ) = 0;
          
           /**
           * @brief This is called by the LayeredCostmap to poll this plugin as to how
           * much of the costmap it needs to update. Each layer can increase
           * the size of this bounds.
           *
           * For more details,   see "Layered Costmaps for Context-Sensitive Navigation",  
           * by Lu et. Al,   IROS 2014.
           */
     100   virtual void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y ) = 0;
          
           /**
           * @brief Actually update the underlying costmap,   only within the bounds
           * calculated during UpdateBounds(   ).
           */
     110   virtual void updateCosts(  
     111   Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j ) = 0;
          
           /** @brief Implement this to make this layer match the size of the parent costmap. */
     115   virtual void matchSize(   ) {}
          
           /** @brief LayeredCostmap calls this whenever the footprint there
           * changes (  via LayeredCostmap::setFootprint(   ) ). Override to be
           * notified of changes to the robot's footprint. */
     120   virtual void onFootprintChanged(   ) {}
           /** @brief Get the name of the costmap layer */
     122   std::string getName(   ) const
           {
           return name_;
           }
          
           /**
           * @brief Check to make sure all the data in the layer is up to date.
           * If the layer is not up to date,   then it may be unsafe to
           * plan using the data from this layer,   and the planner may
           * need to know.
           *
           * A layer's current state should be managed by the protected
           * variable current_.
           * @return Whether the data in the layer is up to date.
           */
     137   bool isCurrent(   ) const
           {
           return current_;
           }
          
           /** @brief Convenience function for layered_costmap_->getFootprint(   ). */
     143   const std::vector<geometry_msgs::msg::Point> & getFootprint(   ) const;
          
           /** @brief Convenience functions for declaring ROS parameters */
     146   void declareParameter(  
     147   const std::string & param_name,  
     148   const rclcpp::ParameterValue & value );
           /** @brief Convenience functions for declaring ROS parameters */
     150   void declareParameter(  
     151   const std::string & param_name,  
     152   const rclcpp::ParameterType & param_type );
           /** @brief Convenience functions for declaring ROS parameters */
     154   bool hasParameter(  const std::string & param_name );
           /** @brief Convenience functions for declaring ROS parameters */
     156   std::string getFullName(  const std::string & param_name );
          
          protected:
     159   LayeredCostmap * layered_costmap_;
     160   std::string name_;
     161   tf2_ros::Buffer * tf_;
     162   rclcpp::CallbackGroup::SharedPtr callback_group_;
     163   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
     164   rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_costmap_2d" )};
          
           /** @brief This is called at the end of initialize(   ). Override to
           * implement subclass-specific initialization.
           *
           * tf_,   name_,   and layered_costmap_ will all be set already when this is called.
           */
     172   virtual void onInitialize(   ) {}
          
           bool current_;
           // Currently this var is managed by subclasses.
           // TODO(  bpwilcox ): make this managed by this class and/or container class.
           bool enabled_;
          
           // Names of the parameters declared on the ROS node
           std::unordered_set<std::string> local_params_;
          
          private:
           std::vector<geometry_msgs::msg::Point> footprint_spec_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_
          #define NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_costmap_2d/layer.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          
          namespace nav2_costmap_2d
          {
      51  class Layer;
          
          /**
           * @class LayeredCostmap
           * @brief Instantiates different layer plugins and aggregates them into one score
           */
      57  class LayeredCostmap
          {
          public:
           /**
           * @brief Constructor for a costmap
           */
      63   LayeredCostmap(  std::string global_frame,   bool rolling_window,   bool track_unknown );
          
           /**
           * @brief Destructor
           */
      68   ~LayeredCostmap(   );
          
           /**
           * @brief Update the underlying costmap with new data.
           * If you want to update the map outside of the update loop that runs,   you can call this.
           */
      74   void updateMap(  double robot_x,   double robot_y,   double robot_yaw );
          
      76   std::string getGlobalFrameID(   ) const
           {
           return global_frame_;
           }
          
           /**
           * @brief Resize the map to a new size,   resolution,   or origin
           */
      84   void resizeMap(  
           unsigned int size_x,   unsigned int size_y,   double resolution,   double origin_x,  
           double origin_y,  
      87   bool size_locked = false );
          
           /**
           * @brief Get the size of the bounds for update
           */
      92   void getUpdatedBounds(  double & minx,   double & miny,   double & maxx,   double & maxy )
           {
           minx = minx_;
           miny = miny_;
           maxx = maxx_;
           maxy = maxy_;
           }
          
           /**
           * @brief If the costmap is current,   e.g. are all the layers processing recent data
           * and not stale information for a good state.
           */
     104   bool isCurrent(   );
          
           /**
           * @brief Get the costmap pointer to the master costmap
           */
     109   Costmap2D * getCostmap(   )
           {
           return &combined_costmap_;
           }
          
           /**
           * @brief If this costmap is rolling or not
           */
     117   bool isRolling(   )
           {
           return rolling_window_;
           }
          
           /**
           * @brief If this costmap is tracking unknown space or not
           */
     125   bool isTrackingUnknown(   )
           {
           return combined_costmap_.getDefaultValue(   ) == nav2_costmap_2d::NO_INFORMATION;
           }
          
           /**
           * @brief Get the vector of pointers to the costmap plugins
           */
           std::vector<std::shared_ptr<Layer>> * getPlugins(   )
           {
           return &plugins_;
           }
          
           /**
           * @brief Get the vector of pointers to the costmap filters
           */
     141   std::vector<std::shared_ptr<Layer>> * getFilters(   )
           {
           return &filters_;
           }
          
           /**
           * @brief Add a new plugin to the plugins vector to process
           */
     149   void addPlugin(  std::shared_ptr<Layer> plugin );
          
          
           /**
           * @brief Add a new costmap filter plugin to the filters vector to process
           */
     155   void addFilter(  std::shared_ptr<Layer> filter )
           {
           filters_.push_back(  filter );
           }
          
           /**
           * @brief Get if the size of the costmap is locked
           */
     163   bool isSizeLocked(   )
           {
           return size_locked_;
           }
          
           /**
           * @brief Get the bounds of the costmap
           */
     171   void getBounds(  unsigned int * x0,   unsigned int * xn,   unsigned int * y0,   unsigned int * yn )
           {
           *x0 = bx0_;
           *xn = bxn_;
           *y0 = by0_;
           *yn = byn_;
           }
          
           /**
           * @brief if the costmap is initialized
           */
     182   bool isInitialized(   )
           {
           return initialized_;
           }
          
           /** @brief Updates the stored footprint,   updates the circumscribed
           * and inscribed radii,   and calls onFootprintChanged(   ) in all
           * layers. */
     190   void setFootprint(  const std::vector<geometry_msgs::msg::Point> & footprint_spec );
          
           /** @brief Returns the latest footprint stored with setFootprint(   ). */
     193   const std::vector<geometry_msgs::msg::Point> & getFootprint(   ) {return footprint_;}
          
           /** @brief The radius of a circle centered at the origin of the
           * robot which just surrounds all points on the robot's
           * footprint.
           *
           * This is updated by setFootprint(   ). */
     200   double getCircumscribedRadius(   ) {return circumscribed_radius_;}
          
           /** @brief The radius of a circle centered at the origin of the
           * robot which is just within all points and edges of the robot's
           * footprint.
           *
           * This is updated by setFootprint(   ). */
     207   double getInscribedRadius(   ) {return inscribed_radius_;}
          
           /** @brief Checks if the robot is outside the bounds of its costmap in the case
           * of poorly configured setups. */
     211   bool isOutofBounds(  double robot_x,   double robot_y );
          
          private:
           // primary_costmap_ is a bottom costmap used by plugins when costmap filters were enabled.
           // combined_costmap_ is a final costmap where all results produced by plugins and filters (  if any )
           // to be merged.
           // The separation is aimed to avoid interferences of work between plugins and filters.
           // primay_costmap_ and combined_costmap_ have the same sizes,   origins and default values.
           Costmap2D primary_costmap_,   combined_costmap_;
           std::string global_frame_;
          
           bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot
          
           bool current_;
           double minx_,   miny_,   maxx_,   maxy_;
           unsigned int bx0_,   bxn_,   by0_,   byn_;
          
           std::vector<std::shared_ptr<Layer>> plugins_;
           std::vector<std::shared_ptr<Layer>> filters_;
          
           bool initialized_;
           bool size_locked_;
           double circumscribed_radius_,   inscribed_radius_;
           std::vector<geometry_msgs::msg::Point> footprint_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

          /*
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Authors: Conor McGann
           */
          
          #ifndef NAV2_COSTMAP_2D__OBSERVATION_HPP_
          #define NAV2_COSTMAP_2D__OBSERVATION_HPP_
          
          #include <geometry_msgs/msg/point.hpp>
          #include <sensor_msgs/msg/point_cloud2.hpp>
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @brief Stores an observation in terms of a point cloud and the origin of the source
           * @note Tried to make members and constructor arguments const but the compiler would not accept the default
           * assignment operator for vector insertion!
           */
      46  class Observation
          {
          public:
           /**
           * @brief Creates an empty observation
           */
           Observation(   )
           : cloud_(  new sensor_msgs::msg::PointCloud2(   ) ),   obstacle_max_range_(  0.0 ),   obstacle_min_range_(  0.0 ),  
           raytrace_max_range_(  0.0 ),  
           raytrace_min_range_(  0.0 )
           {
           }
           /**
           * @brief A destructor
           */
           virtual ~Observation(   )
           {
           delete cloud_;
           }
          
           /**
           * @brief Copy assignment operator
           * @param obs The observation to copy
           */
      70   Observation & operator=(  const Observation & obs )
           {
           origin_ = obs.origin_;
           cloud_ = new sensor_msgs::msg::PointCloud2(  *(  obs.cloud_ ) );
           obstacle_max_range_ = obs.obstacle_max_range_;
           obstacle_min_range_ = obs.obstacle_min_range_;
           raytrace_max_range_ = obs.raytrace_max_range_;
           raytrace_min_range_ = obs.raytrace_min_range_;
          
           return *this;
           }
          
           /**
           * @brief Creates an observation from an origin point and a point cloud
           * @param origin The origin point of the observation
           * @param cloud The point cloud of the observation
           * @param obstacle_max_range The range out to which an observation should be able to insert obstacles
           * @param obstacle_min_range The range from which an observation should be able to insert obstacles
           * @param raytrace_max_range The range out to which an observation should be able to clear via raytracing
           * @param raytrace_min_range The range from which an observation should be able to clear via raytracing
           */
      91   Observation(  
           geometry_msgs::msg::Point & origin,   const sensor_msgs::msg::PointCloud2 & cloud,  
           double obstacle_max_range,   double obstacle_min_range,   double raytrace_max_range,  
           double raytrace_min_range )
           : origin_(  origin ),   cloud_(  new sensor_msgs::msg::PointCloud2(  cloud ) ),  
           obstacle_max_range_(  obstacle_max_range ),   obstacle_min_range_(  obstacle_min_range ),  
           raytrace_max_range_(  raytrace_max_range ),   raytrace_min_range_(  
           raytrace_min_range )
           {
           }
          
           /**
           * @brief Copy constructor
           * @param obs The observation to copy
           */
           Observation(  const Observation & obs )
           : origin_(  obs.origin_ ),   cloud_(  new sensor_msgs::msg::PointCloud2(  *(  obs.cloud_ ) ) ),  
           obstacle_max_range_(  obs.obstacle_max_range_ ),   obstacle_min_range_(  obs.obstacle_min_range_ ),  
           raytrace_max_range_(  obs.raytrace_max_range_ ),  
           raytrace_min_range_(  obs.raytrace_min_range_ )
           {
           }
          
           /**
           * @brief Creates an observation from a point cloud
           * @param cloud The point cloud of the observation
           * @param obstacle_max_range The range out to which an observation should be able to insert obstacles
           * @param obstacle_min_range The range from which an observation should be able to insert obstacles
           */
           Observation(  
           const sensor_msgs::msg::PointCloud2 & cloud,   double obstacle_max_range,  
           double obstacle_min_range )
           : cloud_(  new sensor_msgs::msg::PointCloud2(  cloud ) ),   obstacle_max_range_(  obstacle_max_range ),  
           obstacle_min_range_(  obstacle_min_range ),  
           raytrace_max_range_(  0.0 ),   raytrace_min_range_(  0.0 )
           {
           }
          
           geometry_msgs::msg::Point origin_;
           sensor_msgs::msg::PointCloud2 * cloud_;
           double obstacle_max_range_,   obstacle_min_range_,   raytrace_max_range_,   raytrace_min_range_;
          };
          
          } // namespace nav2_costmap_2d
          #endif // NAV2_COSTMAP_2D__OBSERVATION_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_
          #define NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_
          
          #include <vector>
          #include <list>
          #include <string>
          
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "rclcpp/time.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
          #include "sensor_msgs/msg/point_cloud2.hpp"
          #include "nav2_costmap_2d/observation.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          
          namespace nav2_costmap_2d
          {
          /**
           * @class ObservationBuffer
           * @brief Takes in point clouds from sensors,   transforms them to the desired frame,   and stores them
           */
      59  class ObservationBuffer
          {
          public:
           /**
           * @brief Constructs an observation buffer
           * @param topic_name The topic of the observations,   used as an identifier for error and warning messages
           * @param observation_keep_time Defines the persistence of observations in seconds,   0 means only keep the latest
           * @param expected_update_rate How often this buffer is expected to be updated,   0 means there is no limit
           * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
           * @param max_obstacle_height The minimum height of a hitpoint to be considered legal
           * @param obstacle_max_range The range to which the sensor should be trusted for inserting obstacles
           * @param obstacle_min_range The range from which the sensor should be trusted for inserting obstacles
           * @param raytrace_max_range The range to which the sensor should be trusted for raytracing to clear out space
           * @param raytrace_min_range The range from which the sensor should be trusted for raytracing to clear out space
           * @param tf2_buffer A reference to a tf2 Buffer
           * @param global_frame The frame to transform PointClouds into
           * @param sensor_frame The frame of the origin of the sensor,   can be left blank to be read from the messages
           * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
           */
      78   ObservationBuffer(  
      79   const nav2_util::LifecycleNode::WeakPtr & parent,  
      80   std::string topic_name,  
           double observation_keep_time,  
           double expected_update_rate,  
           double min_obstacle_height,   double max_obstacle_height,   double obstacle_max_range,  
           double obstacle_min_range,  
      85   double raytrace_max_range,   double raytrace_min_range,   tf2_ros::Buffer & tf2_buffer,  
      86   std::string global_frame,  
      87   std::string sensor_frame,  
      88   tf2::Duration tf_tolerance );
          
           /**
           * @brief Destructor... cleans up
           */
      93   ~ObservationBuffer(   );
          
           /**
           * @brief Transforms a PointCloud to the global frame and buffers it
           * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
           * @param cloud The cloud to be buffered
           */
     100   void bufferCloud(  const sensor_msgs::msg::PointCloud2 & cloud );
          
           /**
           * @brief Pushes copies of all current observations onto the end of the vector passed in
           * @param observations The vector to be filled
           */
     106   void getObservations(  std::vector<Observation> & observations );
          
           /**
           * @brief Check if the observation buffer is being update at its expected rate
           * @return True if it is being updated at the expected rate,   false otherwise
           */
     112   bool isCurrent(   ) const;
          
           /**
           * @brief Lock the observation buffer
           */
     117   inline void lock(   )
           {
           lock_.lock(   );
           }
          
           /**
           * @brief Lock the observation buffer
           */
     125   inline void unlock(   )
           {
           lock_.unlock(   );
           }
          
           /**
           * @brief Reset last updated timestamp
           */
     133   void resetLastUpdated(   );
          
          private:
           /**
           * @brief Removes any stale observations from the buffer list
           */
     139   void purgeStaleObservations(   );
          
     141   rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_costmap_2d" )};
           tf2_ros::Buffer & tf2_buffer_;
           const rclcpp::Duration observation_keep_time_;
           const rclcpp::Duration expected_update_rate_;
           rclcpp::Time last_updated_;
           std::string global_frame_;
           std::string sensor_frame_;
           std::list<Observation> observation_list_;
           std::string topic_name_;
           double min_obstacle_height_,   max_obstacle_height_;
           std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
           double obstacle_max_range_,   obstacle_min_range_,   raytrace_max_range_,   raytrace_min_range_;
           tf2::Duration tf_tolerance_;
          };
          } // namespace nav2_costmap_2d
          #endif // NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_
          #define NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "laser_geometry/laser_geometry.hpp"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wreorder"
          #include "tf2_ros/message_filter.h"
          #pragma GCC diagnostic pop
          #include "message_filters/subscriber.h"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "sensor_msgs/msg/laser_scan.hpp"
          #include "sensor_msgs/msg/point_cloud.hpp"
          #include "sensor_msgs/msg/point_cloud2.hpp"
          #include "nav2_costmap_2d/costmap_layer.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/observation_buffer.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class ObstacleLayer
           * @brief Takes in laser and pointcloud data to populate into 2D costmap
           */
      68  class ObstacleLayer : public CostmapLayer
          {
          public:
           /**
           * @brief A constructor
           */
      74   ObstacleLayer(   )
           {
           costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
           }
          
           /**
           * @brief A destructor
           */
      82   virtual ~ObstacleLayer(   );
           /**
           * @brief Initialization process of layer on startup
           */
      86   virtual void onInitialize(   );
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
      97   virtual void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y );
           /**
           * @brief Update the costs in the master costmap in the window
           * @param master_grid The master costmap grid to update
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
     110   virtual void updateCosts(  
     111   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j );
          
           /**
           * @brief Deactivate the layer
           */
     117   virtual void deactivate(   );
          
           /**
           * @brief Activate the layer
           */
     122   virtual void activate(   );
          
           /**
           * @brief Reset this costmap
           */
     127   virtual void reset(   );
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
     132   virtual bool isClearable(   ) {return true;}
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
     139   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           /**
           * @brief triggers the update of observations buffer
           */
     144   void resetBuffersLastUpdated(   );
          
           /**
           * @brief A callback to handle buffering LaserScan messages
           * @param message The message returned from a message notifier
           * @param buffer A pointer to the observation buffer to update
           */
     151   void laserScanCallback(  
     152   sensor_msgs::msg::LaserScan::ConstSharedPtr message,  
     153   const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
          
           /**
           * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
           * @param message The message returned from a message notifier
           * @param buffer A pointer to the observation buffer to update
           */
     160   void laserScanValidInfCallback(  
     161   sensor_msgs::msg::LaserScan::ConstSharedPtr message,  
     162   const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
          
           /**
           * @brief A callback to handle buffering PointCloud2 messages
           * @param message The message returned from a message notifier
           * @param buffer A pointer to the observation buffer to update
           */
     169   void pointCloud2Callback(  
     170   sensor_msgs::msg::PointCloud2::ConstSharedPtr message,  
     171   const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
          
           // for testing purposes
     174   void addStaticObservation(  nav2_costmap_2d::Observation & obs,   bool marking,   bool clearing );
     175   void clearStaticObservations(  bool marking,   bool clearing );
          
          protected:
           /**
           * @brief Get the observations used to mark space
           * @param marking_observations A reference to a vector that will be populated with the observations
           * @return True if all the observation buffers are current,   false otherwise
           */
     183   bool getMarkingObservations(  
     184   std::vector<nav2_costmap_2d::Observation> & marking_observations ) const;
          
           /**
           * @brief Get the observations used to clear space
           * @param clearing_observations A reference to a vector that will be populated with the observations
           * @return True if all the observation buffers are current,   false otherwise
           */
     191   bool getClearingObservations(  
     192   std::vector<nav2_costmap_2d::Observation> & clearing_observations ) const;
          
           /**
           * @brief Clear freespace based on one observation
           * @param clearing_observation The observation used to raytrace
           * @param min_x
           * @param min_y
           * @param max_x
           * @param max_y
           */
     202   virtual void raytraceFreespace(  
     203   const nav2_costmap_2d::Observation & clearing_observation,  
           double * min_x,   double * min_y,  
           double * max_x,  
           double * max_y );
          
           /**
           * @brief Process update costmap with raytracing the window bounds
           */
     211   void updateRaytraceBounds(  
           double ox,   double oy,   double wx,   double wy,   double max_range,   double min_range,  
           double * min_x,   double * min_y,  
           double * max_x,  
           double * max_y );
          
     217   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
     218   bool footprint_clearing_enabled_;
           /**
           * @brief Clear costmap layer info below the robot's footprint
           */
     222   void updateFootprint(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y );
          
     228   std::string global_frame_; ///< @brief The global frame for the costmap
           double max_obstacle_height_; ///< @brief Max Obstacle Height
          
           /// @brief Used to project laser scans into point clouds
     232   laser_geometry::LaserProjection projector_;
           /// @brief Used for the observation message filters
           std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>>
           observation_subscribers_;
           /// @brief Used to make sure that transforms are available for each sensor
           std::vector<std::shared_ptr<tf2_ros::MessageFilterBase>> observation_notifiers_;
           /// @brief Used to store observations from various sensors
           std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> observation_buffers_;
           /// @brief Used to store observation buffers used for marking obstacles
           std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> marking_buffers_;
           /// @brief Used to store observation buffers used for clearing obstacles
           std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> clearing_buffers_;
          
           /// @brief Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          
           // Used only for testing purposes
           std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
           std::vector<nav2_costmap_2d::Observation> static_marking_observations_;
          
           bool rolling_window_;
           bool was_reset_;
           int combination_method_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018 David V. Lu!!
           * Copyright (  c ) 2020,   Bytes Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
          #define NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
          
          #include <list>
          #include <string>
          #include <vector>
          #include <mutex>
          
          #include "map_msgs/msg/occupancy_grid_update.hpp"
          #include "message_filters/subscriber.h"
          #include "nav2_costmap_2d/costmap_layer.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "sensor_msgs/msg/range.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class RangeSensorLayer
           * @brief Takes in IR/Sonar/similar point measurement sensors and populates in costmap
           */
      60  class RangeSensorLayer : public CostmapLayer
          {
          public:
           enum class InputSensorType
           {
           VARIABLE,  
           FIXED,  
           ALL
           };
          
           /**
           * @brief A constructor
           */
      73   RangeSensorLayer(   );
          
           /**
           * @brief Initialization process of layer on startup
           */
      78   virtual void onInitialize(   );
          
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           virtual void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,  
           double * min_x,   double * min_y,   double * max_x,   double * max_y );
          
           /**
           * @brief Update the costs in the master costmap in the window
           * @param master_grid The master costmap grid to update
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
           virtual void updateCosts(  
           nav2_costmap_2d::Costmap2D & master_grid,   int min_i,  
           int min_j,   int max_i,   int max_j );
          
           /**
           * @brief Reset this costmap
           */
           virtual void reset(   );
          
           /**
           * @brief Deactivate the layer
           */
           virtual void deactivate(   );
          
           /**
           * @brief Activate the layer
           */
           virtual void activate(   );
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
           virtual bool isClearable(   ) {return true;}
          
           /**
           * @brief Handle an incoming Range message to populate into costmap
           */
           void bufferIncomingRangeMsg(  const sensor_msgs::msg::Range::SharedPtr range_message );
          
          private:
           /**
           * @brief Processes all sensors into the costmap buffered from callbacks
           */
           void updateCostmap(   );
           /**
           * @brief Update the actual costmap with the values processed
           */
           void updateCostmap(  sensor_msgs::msg::Range & range_message,   bool clear_sensor_cone );
          
           /**
           * @brief Process general incoming range sensor data. If min=max ranges,  
           * fixed processor callback is used,   else uses variable callback
           */
           void processRangeMsg(  sensor_msgs::msg::Range & range_message );
           /**
           * @brief Process fixed range incoming range sensor data
           */
           void processFixedRangeMsg(  sensor_msgs::msg::Range & range_message );
           /**
           * @brief Process variable range incoming range sensor data
           */
           void processVariableRangeMsg(  sensor_msgs::msg::Range & range_message );
          
           /**
           * @brief Reset the angle min/max x,   and min/max y values
           */
           void resetRange(   );
          
           /**
           * @brief Get the gamma value for an angle,   theta
           */
           inline double gamma(  double theta );
           /**
           * @brief Get the delta value for an angle,   phi
           */
           inline double delta(  double phi );
           /**
           * @brief Apply the sensor model of the layer for range sensors
           */
           inline double sensor_model(  double r,   double phi,   double theta );
          
           /**
           * @brief Get angles
           */
           inline void get_deltas(  double angle,   double * dx,   double * dy );
          
           /**
           * @brief Update the cost in a cell with information
           */
           inline void update_cell(  
           double ox,   double oy,   double ot,  
           double r,   double nx,   double ny,   bool clear );
          
           /**
           * @brief Find probability value of a cost
           */
           inline double to_prob(  unsigned char c )
           {
           return static_cast<double>(  c ) / nav2_costmap_2d::LETHAL_OBSTACLE;
           }
          
           /**
           * @brief Find cost value of a probability
           */
           inline unsigned char to_cost(  double p )
           {
           return static_cast<unsigned char>(  p * nav2_costmap_2d::LETHAL_OBSTACLE );
           }
          
           std::function<void(  sensor_msgs::msg::Range & range_message )> processRangeMessageFunc_;
           std::mutex range_message_mutex_;
           std::list<sensor_msgs::msg::Range> range_msgs_buffer_;
          
           double max_angle_,   phi_v_;
           double inflate_cone_;
           std::string global_frame_;
          
           double clear_threshold_,   mark_threshold_;
           bool clear_on_max_reading_;
           bool was_reset_;
          
           tf2::Duration transform_tolerance_;
           double no_readings_timeout_;
           rclcpp::Time last_reading_time_;
           unsigned int buffered_readings_;
           std::vector<rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr> range_subs_;
           double min_x_,   min_y_,   max_x_,   max_y_;
          
           /**
           * @brief Find the area of 3 points of a triangle
           */
           float area(  int x1,   int y1,   int x2,   int y2,   int x3,   int y3 )
           {
           return fabs(  (  x1 * (  y2 - y3 ) + x2 * (  y3 - y1 ) + x3 * (  y1 - y2 ) ) / 2.0 );
           }
          
           /**
           * @brief Find the cross product of 3 vectors,   A,  B,  C
           */
           int orient2d(  int Ax,   int Ay,   int Bx,   int By,   int Cx,   int Cy )
           {
           return (  Bx - Ax ) * (  Cy - Ay ) - (  By - Ay ) * (  Cx - Ax );
           }
          };
          } // namespace nav2_costmap_2d
          #endif // NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__STATIC_LAYER_HPP_
          #define NAV2_COSTMAP_2D__STATIC_LAYER_HPP_
          
          #include <mutex>
          #include <string>
          #include <vector>
          
          #include "map_msgs/msg/occupancy_grid_update.hpp"
          #include "message_filters/subscriber.h"
          #include "nav2_costmap_2d/costmap_layer.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class StaticLayer
           * @brief Takes in a map generated from SLAM to add costs to costmap
           */
      59  class StaticLayer : public CostmapLayer
          {
          public:
           /**
           * @brief Static Layer constructor
           */
      65   StaticLayer(   );
           /**
           * @brief Static Layer destructor
           */
      69   virtual ~StaticLayer(   );
          
           /**
           * @brief Initialization process of layer on startup
           */
      74   virtual void onInitialize(   );
          
           /**
           * @brief Activate this layer
           */
      79   virtual void activate(   );
           /**
           * @brief Deactivate this layer
           */
      83   virtual void deactivate(   );
          
           /**
           * @brief Reset this costmap
           */
      88   virtual void reset(   );
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
      93   virtual bool isClearable(   ) {return false;}
          
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
     105   virtual void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,   double * max_x,   double * max_y );
          
           /**
           * @brief Update the costs in the master costmap in the window
           * @param master_grid The master costmap grid to update
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
     117   virtual void updateCosts(  
     118   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j );
          
           /**
           * @brief Match the size of the master costmap
           */
     124   virtual void matchSize(   );
          
          private:
           /**
           * @brief Get parameters of layer
           */
     130   void getParameters(   );
          
           /**
           * @brief Process a new map coming from a topic
           */
     135   void processMap(  const nav_msgs::msg::OccupancyGrid & new_map );
          
           /**
           * @brief Callback to update the costmap's map from the map_server
           * @param new_map The map to put into the costmap. The origin of the new
           * map along with its size will determine what parts of the costmap's
           * static map are overwritten.
           */
     143   void incomingMap(  const nav_msgs::msg::OccupancyGrid::SharedPtr new_map );
           /**
           * @brief Callback to update the costmap's map from the map_server (  or SLAM )
           * with an update in a particular area of the map
           */
     148   void incomingUpdate(  map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update );
          
           /**
           * @brief Interpret the value in the static map given on the topic to
           * convert into costs for the costmap to utilize
           */
     154   unsigned char interpretValue(  unsigned char value );
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
     161   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
     163   std::string global_frame_; ///< @brief The global frame for the costmap
     164   std::string map_frame_; /// @brief frame that map is located in
          
           bool has_updated_data_{false};
          
           unsigned int x_{0};
           unsigned int y_{0};
           unsigned int width_{0};
           unsigned int height_{0};
          
           rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
           rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
          
           // Parameters
           std::string map_topic_;
           bool map_subscribe_transient_local_;
           bool subscribe_to_updates_;
           bool track_unknown_space_;
           bool use_maximum_;
           unsigned char lethal_threshold_;
           unsigned char unknown_cost_value_;
           bool trinary_costmap_;
           bool map_received_{false};
           tf2::Duration transform_tolerance_;
           std::atomic<bool> update_in_progress_;
           nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__STATIC_LAYER_HPP_

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #ifndef NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_
          #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_
          
          #include <vector>
          #include "message_filters/subscriber.h"
          
          #include <rclcpp/rclcpp.hpp>
          #include <nav2_costmap_2d/layer.hpp>
          #include <nav2_costmap_2d/layered_costmap.hpp>
          #include <nav2_costmap_2d/observation_buffer.hpp>
          #include <nav_msgs/msg/occupancy_grid.hpp>
          #include <nav2_msgs/msg/voxel_grid.hpp>
          #include <sensor_msgs/msg/laser_scan.hpp>
          #include <laser_geometry/laser_geometry.hpp>
          #include <sensor_msgs/msg/point_cloud.hpp>
          #include <sensor_msgs/msg/point_cloud2.hpp>
          #include <nav2_costmap_2d/obstacle_layer.hpp>
          #include <nav2_voxel_grid/voxel_grid.hpp>
          
          namespace nav2_costmap_2d
          {
          
          /**
           * @class VoxelLayer
           * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment
           */
      64  class VoxelLayer : public ObstacleLayer
          {
          public:
           /**
           * @brief Voxel Layer constructor
           */
      70   VoxelLayer(   )
           : voxel_grid_(  0,   0,   0 )
           {
           costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D
           }
          
           /**
           * @brief Voxel Layer destructor
           */
      79   virtual ~VoxelLayer(   );
          
           /**
           * @brief Initialization process of layer on startup
           */
      84   virtual void onInitialize(   );
          
           /**
           * @brief Update the bounds of the master costmap by this layer's update dimensions
           * @param robot_x X pose of robot
           * @param robot_y Y pose of robot
           * @param robot_yaw Robot orientation
           * @param min_x X min map coord of the window to update
           * @param min_y Y min map coord of the window to update
           * @param max_x X max map coord of the window to update
           * @param max_y Y max map coord of the window to update
           */
      96   virtual void updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y );
          
           /**
           * @brief Update the layer's origin to a new pose,   often when in a rolling costmap
           */
     105   void updateOrigin(  double new_origin_x,   double new_origin_y );
          
           /**
           * @brief If layer is discretely populated
           */
     110   bool isDiscretized(   )
           {
           return true;
           }
          
           /**
           * @brief Match the size of the master costmap
           */
     118   virtual void matchSize(   );
          
           /**
           * @brief Reset this costmap
           */
     123   virtual void reset(   );
          
           /**
           * @brief If clearing operations should be processed on this layer or not
           */
     128   virtual bool isClearable(   ) {return true;}
          
          protected:
           /**
           * @brief Reset internal maps
           */
     134   virtual void resetMaps(   );
          
          private:
           /**
           * @brief Use raycasting between 2 points to clear freespace
           */
     140   virtual void raytraceFreespace(  
     141   const nav2_costmap_2d::Observation & clearing_observation,  
           double * min_x,   double * min_y,  
           double * max_x,  
           double * max_y );
          
     146   bool publish_voxel_;
     147   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
     148   nav2_voxel_grid::VoxelGrid voxel_grid_;
           double z_resolution_,   origin_z_;
           int unknown_threshold_,   mark_threshold_,   size_z_;
     151   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr
           clearing_endpoints_pub_;
          
           /**
           * @brief Covert world coordinates into map coordinates
           */
     157   inline bool worldToMap3DFloat(  
           double wx,   double wy,   double wz,   double & mx,   double & my,  
           double & mz )
           {
           if (  wx < origin_x_ || wy < origin_y_ || wz < origin_z_ ) {
           return false;
           }
           mx = (  (  wx - origin_x_ ) / resolution_ );
           my = (  (  wy - origin_y_ ) / resolution_ );
           mz = (  (  wz - origin_z_ ) / z_resolution_ );
           if (  mx < size_x_ && my < size_y_ && mz < size_z_ ) {
           return true;
           }
          
           return false;
           }
          
           /**
           * @brief Covert world coordinates into map coordinates
           */
     177   inline bool worldToMap3D(  
           double wx,   double wy,   double wz,   unsigned int & mx,   unsigned int & my,  
           unsigned int & mz )
           {
           if (  wx < origin_x_ || wy < origin_y_ || wz < origin_z_ ) {
           return false;
           }
          
           mx = static_cast<unsigned int>(  (  wx - origin_x_ ) / resolution_ );
           my = static_cast<unsigned int>(  (  wy - origin_y_ ) / resolution_ );
           mz = static_cast<unsigned int>(  (  wz - origin_z_ ) / z_resolution_ );
          
           if (  mx < size_x_ && my < size_y_ && mz < (  unsigned int )size_z_ ) {
           return true;
           }
          
           return false;
           }
          
           /**
           * @brief Covert map coordinates into world coordinates
           */
     199   inline void mapToWorld3D(  
           unsigned int mx,   unsigned int my,   unsigned int mz,   double & wx,  
           double & wy,  
           double & wz )
           {
           // returns the center point of the cell
           wx = origin_x_ + (  mx + 0.5 ) * resolution_;
           wy = origin_y_ + (  my + 0.5 ) * resolution_;
           wz = origin_z_ + (  mz + 0.5 ) * z_resolution_;
           }
          
           /**
           * @brief Find L2 norm distance in 3D
           */
     213   inline double dist(  double x0,   double y0,   double z0,   double x1,   double y1,   double z1 )
           {
           return sqrt(  (  x1 - x0 ) * (  x1 - x0 ) + (  y1 - y0 ) * (  y1 - y0 ) + (  z1 - z0 ) * (  z1 - z0 ) );
           }
          
           /**
           * @brief Get the height of the voxel sizes in meters
           */
     221   double getSizeInMetersZ(   ) const
           {
           return (  size_z_ - 1 + 0.5 ) * z_resolution_;
           }
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
     231   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           // Dynamic parameters handler
     234   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_costmap_2d
          
          #endif // NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_

navigation2/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
          
          #include <exception>
          
          namespace nav2_costmap_2d
          {
          
      45  CostmapFilter::CostmapFilter(   )
          : filter_info_topic_(  "" ),   mask_topic_(  "" )
          {
           access_ = new mutex_t(   );
          }
          
      51  CostmapFilter::~CostmapFilter(   )
          {
           delete access_;
          }
          
      56  void CostmapFilter::onInitialize(   )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           try {
           // Declare common for all costmap filters parameters
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "filter_info_topic",   rclcpp::PARAMETER_STRING );
           declareParameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.1 ) );
          
           // Get parameters
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           filter_info_topic_ = node->get_parameter(  name_ + "." + "filter_info_topic" ).as_string(   );
           double transform_tolerance;
           node->get_parameter(  name_ + "." + "transform_tolerance",   transform_tolerance );
           transform_tolerance_ = tf2::durationFromSec(  transform_tolerance );
           } catch (  const std::exception & ex ) {
           RCLCPP_ERROR(  logger_,   "Parameter problem: %s",   ex.what(   ) );
           throw ex;
           }
          }
          
      81  void CostmapFilter::activate(   )
          {
           initializeFilter(  filter_info_topic_ );
          }
          
      86  void CostmapFilter::deactivate(   )
          {
           resetFilter(   );
          }
          
      91  void CostmapFilter::reset(   )
          {
           resetFilter(   );
           initializeFilter(  filter_info_topic_ );
           current_ = false;
          }
          
      98  void CostmapFilter::updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,  
           double * /*min_x*/,   double * /*min_y*/,   double * /*max_x*/,   double * /*max_y*/ )
          {
           if (  !enabled_ ) {
           return;
           }
          
           latest_pose_.x = robot_x;
           latest_pose_.y = robot_y;
           latest_pose_.theta = robot_yaw;
          }
          
     111  void CostmapFilter::updateCosts(  
     112   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
          
           process(  master_grid,   min_i,   min_j,   max_i,   max_j,   latest_pose_ );
           current_ = true;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #include <string>
          #include <memory>
          #include <algorithm>
          #include "tf2/convert.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          
          #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          namespace nav2_costmap_2d
          {
          
      50  KeepoutFilter::KeepoutFilter(   )
          : filter_info_sub_(  nullptr ),   mask_sub_(  nullptr ),   mask_costmap_(  nullptr ),  
           mask_frame_(  "" ),   global_frame_(  "" )
          {
          }
          
      56  void KeepoutFilter::initializeFilter(  
      57   const std::string & filter_info_topic )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           filter_info_topic_ = filter_info_topic;
           // Setting new costmap filter info subscriber
           RCLCPP_INFO(  
           logger_,  
           "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",  
           filter_info_topic_.c_str(   ) );
           filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(  
           filter_info_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &KeepoutFilter::filterInfoCallback,   this,   std::placeholders::_1 ) );
          
           global_frame_ = layered_costmap_->getGlobalFrameID(   );
          }
          
      79  void KeepoutFilter::filterInfoCallback(  
      80   const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           if (  !mask_sub_ ) {
           RCLCPP_INFO(  
           logger_,  
           "KeepoutFilter: Received filter info from %s topic.",   filter_info_topic_.c_str(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",  
           filter_info_topic_.c_str(   ) );
           // Resetting previous subscriber each time when new costmap filter information arrives
           mask_sub_.reset(   );
           }
          
           // Checking that base and multiplier are set to their default values
           if (  msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT ) {
           RCLCPP_ERROR(  
           logger_,  
           "KeepoutFilter: For proper use of keepout filter base and multiplier"
           " in CostmapFilterInfo message should be set to their default values (  %f and %f )",  
           BASE_DEFAULT,   MULTIPLIER_DEFAULT );
           }
          
           mask_topic_ = msg->filter_mask_topic;
          
           // Setting new filter mask subscriber
           RCLCPP_INFO(  
           logger_,  
           "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",  
           mask_topic_.c_str(   ) );
           mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(  
           mask_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &KeepoutFilter::maskCallback,   this,   std::placeholders::_1 ) );
          }
          
     123  void KeepoutFilter::maskCallback(  
     124   const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           if (  !mask_costmap_ ) {
           RCLCPP_INFO(  
           logger_,  
           "KeepoutFilter: Received filter mask from %s topic.",   mask_topic_.c_str(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",  
           mask_topic_.c_str(   ) );
           mask_costmap_.reset(   );
           }
          
           // Making a new mask_costmap_
           mask_costmap_ = std::make_unique<Costmap2D>(  *msg );
           mask_frame_ = msg->header.frame_id;
          }
          
     150  void KeepoutFilter::process(  
     151   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j,  
     153   const geometry_msgs::msg::Pose2D & /*pose*/ )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           if (  !mask_costmap_ ) {
           // Show warning message every 2 seconds to not litter an output
           RCLCPP_WARN_THROTTLE(  
           logger_,   *(  clock_ ),   2000,  
           "KeepoutFilter: Filter mask was not received" );
           return;
           }
          
           tf2::Transform tf2_transform;
           tf2_transform.setIdentity(   ); // initialize by identical transform
           int mg_min_x,   mg_min_y; // masger_grid indexes of bottom-left window corner
           int mg_max_x,   mg_max_y; // masger_grid indexes of top-right window corner
          
           if (  mask_frame_ != global_frame_ ) {
           // Filter mask and current layer are in different frames:
           // prepare frame transformation if mask_frame_ != global_frame_
           geometry_msgs::msg::TransformStamped transform;
           try {
           transform = tf_->lookupTransform(  
           mask_frame_,   global_frame_,   tf2::TimePointZero,  
           transform_tolerance_ );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  
           logger_,  
           "KeepoutFilter: Failed to get costmap frame (  %s ) "
           "transformation to mask frame (  %s ) with error: %s",  
           global_frame_.c_str(   ),   mask_frame_.c_str(   ),   ex.what(   ) );
           return;
           }
           tf2::fromMsg(  transform.transform,   tf2_transform );
          
           mg_min_x = min_i;
           mg_min_y = min_j;
           mg_max_x = max_i;
           mg_max_y = max_j;
           } else {
           // Filter mask and current layer are in the same frame:
           // apply the following optimization - iterate only in overlapped
           // (  min_i,   min_j )..(  max_i,   max_j ) & mask_costmap_ area.
           //
           // mask_costmap_
           // *----------------------------*
           // | |
           // | |
           // | (  2 ) |
           // *-----+-------* |
           // | |///////|<- overlapped area |
           // | |///////| to iterate in |
           // | *-------+--------------------*
           // | (  1 ) |
           // | |
           // *-------------*
           // master_grid (  min_i,   min_j )..(  max_i,   max_j ) window
           //
           // ToDo: after costmap rotation will be added,   this should be re-worked.
          
           double wx,   wy; // world coordinates
          
           // Calculating bounds corresponding to bottom-left overlapping (  1 ) corner
           // mask_costmap_ -> master_grid intexes conversion
           const double half_cell_size = 0.5 * mask_costmap_->getResolution(   );
           wx = mask_costmap_->getOriginX(   ) + half_cell_size;
           wy = mask_costmap_->getOriginY(   ) + half_cell_size;
           master_grid.worldToMapNoBounds(  wx,   wy,   mg_min_x,   mg_min_y );
           // Calculation of (  1 ) corner bounds
           if (  mg_min_x >= max_i || mg_min_y >= max_j ) {
           // There is no overlapping. Do nothing.
           return;
           }
           mg_min_x = std::max(  min_i,   mg_min_x );
           mg_min_y = std::max(  min_j,   mg_min_y );
          
           // Calculating bounds corresponding to top-right window (  2 ) corner
           // mask_costmap_ -> master_grid intexes conversion
           wx = mask_costmap_->getOriginX(   ) +
           mask_costmap_->getSizeInCellsX(   ) * mask_costmap_->getResolution(   ) + half_cell_size;
           wy = mask_costmap_->getOriginY(   ) +
           mask_costmap_->getSizeInCellsY(   ) * mask_costmap_->getResolution(   ) + half_cell_size;
           master_grid.worldToMapNoBounds(  wx,   wy,   mg_max_x,   mg_max_y );
           // Calculation of (  2 ) corner bounds
           if (  mg_max_x <= min_i || mg_max_y <= min_j ) {
           // There is no overlapping. Do nothing.
           return;
           }
           mg_max_x = std::min(  max_i,   mg_max_x );
           mg_max_y = std::min(  max_j,   mg_max_y );
           }
          
           // unsigned<-signed conversions.
           unsigned const int mg_min_x_u = static_cast<unsigned int>(  mg_min_x );
           unsigned const int mg_min_y_u = static_cast<unsigned int>(  mg_min_y );
           unsigned const int mg_max_x_u = static_cast<unsigned int>(  mg_max_x );
           unsigned const int mg_max_y_u = static_cast<unsigned int>(  mg_max_y );
          
           unsigned int i,   j; // master_grid iterators
           unsigned int index; // corresponding index of master_grid
           double gl_wx,   gl_wy; // world coordinates in a global_frame_
           double msk_wx,   msk_wy; // world coordinates in a mask_frame_
           unsigned int mx,   my; // mask_costmap_ coordinates
           unsigned char data,   old_data; // master_grid element data
          
           // Main master_grid updating loop
           // Iterate in costmap window by master_grid indexes
           unsigned char * master_array = master_grid.getCharMap(   );
           for (  i = mg_min_x_u; i < mg_max_x_u; i++ ) {
           for (  j = mg_min_y_u; j < mg_max_y_u; j++ ) {
           index = master_grid.getIndex(  i,   j );
           old_data = master_array[index];
           // Calculating corresponding to (  i,   j ) point at mask_costmap_:
           // Get world coordinates in global_frame_
           master_grid.mapToWorld(  i,   j,   gl_wx,   gl_wy );
           if (  mask_frame_ != global_frame_ ) {
           // Transform (  i,   j ) point from global_frame_ to mask_frame_
           tf2::Vector3 point(  gl_wx,   gl_wy,   0 );
           point = tf2_transform * point;
           msk_wx = point.x(   );
           msk_wy = point.y(   );
           } else {
           // In this case master_grid and filter-mask are in the same frame
           msk_wx = gl_wx;
           msk_wy = gl_wy;
           }
           // Get mask coordinates corresponding to (  i,   j ) point at mask_costmap_
           if (  mask_costmap_->worldToMap(  msk_wx,   msk_wy,   mx,   my ) ) {
           data = mask_costmap_->getCost(  mx,   my );
           // Update if mask_ data is valid and greater than existing master_grid's one
           if (  data == NO_INFORMATION ) {
           continue;
           }
           if (  data > old_data || old_data == NO_INFORMATION ) {
           master_array[index] = data;
           }
           }
           }
           }
          }
          
     294  void KeepoutFilter::resetFilter(   )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           filter_info_sub_.reset(   );
           mask_sub_.reset(   );
          }
          
     302  bool KeepoutFilter::isActive(   )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           if (  mask_costmap_ ) {
           return true;
           }
           return false;
          }
          
          } // namespace nav2_costmap_2d
          
          #include "pluginlib/class_list_macros.hpp"
     315  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::KeepoutFilter,   nav2_costmap_2d::Layer )

navigation2/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020 Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           *********************************************************************/
          
          #include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
          
          #include <cmath>
          #include <utility>
          #include <memory>
          #include <string>
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "geometry_msgs/msg/point_stamped.hpp"
          
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          namespace nav2_costmap_2d
          {
          
      52  SpeedFilter::SpeedFilter(   )
          : filter_info_sub_(  nullptr ),   mask_sub_(  nullptr ),  
           speed_limit_pub_(  nullptr ),   filter_mask_(  nullptr ),   mask_frame_(  "" ),   global_frame_(  "" ),  
           speed_limit_(  NO_SPEED_LIMIT ),   speed_limit_prev_(  NO_SPEED_LIMIT )
          {
          }
          
      59  void SpeedFilter::initializeFilter(  
      60   const std::string & filter_info_topic )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           // Declare "speed_limit_topic" parameter specific to SpeedFilter only
           std::string speed_limit_topic;
           declareParameter(  "speed_limit_topic",   rclcpp::ParameterValue(  "speed_limit" ) );
           node->get_parameter(  name_ + "." + "speed_limit_topic",   speed_limit_topic );
          
           filter_info_topic_ = filter_info_topic;
           // Setting new costmap filter info subscriber
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Subscribing to \"%s\" topic for filter info...",  
           filter_info_topic_.c_str(   ) );
           filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(  
           filter_info_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &SpeedFilter::filterInfoCallback,   this,   std::placeholders::_1 ) );
          
           // Get global frame required for speed limit publisher
           global_frame_ = layered_costmap_->getGlobalFrameID(   );
          
           // Create new speed limit publisher
           speed_limit_pub_ = node->create_publisher<nav2_msgs::msg::SpeedLimit>(  
           speed_limit_topic,   rclcpp::QoS(  10 ) );
           speed_limit_pub_->on_activate(   );
          
           // Reset speed conversion states
           base_ = BASE_DEFAULT;
           multiplier_ = MULTIPLIER_DEFAULT;
           percentage_ = false;
          }
          
      98  void SpeedFilter::filterInfoCallback(  
      99   const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           if (  !mask_sub_ ) {
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Received filter info from %s topic.",   filter_info_topic_.c_str(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.",  
           filter_info_topic_.c_str(   ) );
           // Resetting previous subscriber each time when new costmap filter information arrives
           mask_sub_.reset(   );
           }
          
           // Set base_/multiplier_ or use speed limit in % of maximum speed
           base_ = msg->base;
           multiplier_ = msg->multiplier;
           if (  msg->type == SPEED_FILTER_PERCENT ) {
           // Using speed limit in % of maximum speed
           percentage_ = true;
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Using expressed in a percent from maximum speed"
           "speed_limit = %f + filter_mask_data * %f",  
           base_,   multiplier_ );
           } else if (  msg->type == SPEED_FILTER_ABSOLUTE ) {
           // Using speed limit in m/s
           percentage_ = false;
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",  
           base_,   multiplier_ );
           } else {
           RCLCPP_ERROR(  logger_,   "SpeedFilter: Mode is not supported" );
           return;
           }
          
           mask_topic_ = msg->filter_mask_topic;
          
           // Setting new filter mask subscriber
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Subscribing to \"%s\" topic for filter mask...",  
           mask_topic_.c_str(   ) );
           mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(  
           mask_topic_,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &SpeedFilter::maskCallback,   this,   std::placeholders::_1 ) );
          }
          
     156  void SpeedFilter::maskCallback(  
     157   const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           if (  !filter_mask_ ) {
           RCLCPP_INFO(  
           logger_,  
           "SpeedFilter: Received filter mask from %s topic.",   mask_topic_.c_str(   ) );
           } else {
           RCLCPP_WARN(  
           logger_,  
           "SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.",  
           mask_topic_.c_str(   ) );
           filter_mask_.reset(   );
           }
          
           filter_mask_ = msg;
           mask_frame_ = msg->header.frame_id;
          }
          
     177  bool SpeedFilter::transformPose(  
     178   const geometry_msgs::msg::Pose2D & pose,  
     179   geometry_msgs::msg::Pose2D & mask_pose ) const
          {
           if (  mask_frame_ != global_frame_ ) {
           // Filter mask and current layer are in different frames:
           // Transform (  pose.x,   pose.y ) point from current layer frame (  global_frame_ )
           // to mask_pose point in mask_frame_
           geometry_msgs::msg::TransformStamped transform;
           geometry_msgs::msg::PointStamped in,   out;
           in.header.stamp = clock_->now(   );
           in.header.frame_id = global_frame_;
           in.point.x = pose.x;
           in.point.y = pose.y;
           in.point.z = 0;
          
           try {
           tf_->transform(  in,   out,   mask_frame_,   transform_tolerance_ );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  
           logger_,  
           "SpeedFilter: failed to get costmap frame (  %s ) "
           "transformation to mask frame (  %s ) with error: %s",  
           global_frame_.c_str(   ),   mask_frame_.c_str(   ),   ex.what(   ) );
           return false;
           }
           mask_pose.x = out.point.x;
           mask_pose.y = out.point.y;
           } else {
           // Filter mask and current layer are in the same frame:
           // Just use pose coordinates
           mask_pose = pose;
           }
          
           return true;
          }
          
     214  bool SpeedFilter::worldToMask(  double wx,   double wy,   unsigned int & mx,   unsigned int & my ) const
          {
           double origin_x = filter_mask_->info.origin.position.x;
           double origin_y = filter_mask_->info.origin.position.y;
           double resolution = filter_mask_->info.resolution;
           unsigned int size_x = filter_mask_->info.width;
           unsigned int size_y = filter_mask_->info.height;
          
           if (  wx < origin_x || wy < origin_y ) {
           return false;
           }
          
           mx = std::round(  (  wx - origin_x ) / resolution );
           my = std::round(  (  wy - origin_y ) / resolution );
           if (  mx >= size_x || my >= size_y ) {
           return false;
           }
          
           return true;
          }
          
     235  inline int8_t SpeedFilter::getMaskData(  
           const unsigned int mx,   const unsigned int my ) const
          {
           return filter_mask_->data[my * filter_mask_->info.width + mx];
          }
          
     241  void SpeedFilter::process(  
     242   nav2_costmap_2d::Costmap2D & /*master_grid*/,  
           int /*min_i*/,   int /*min_j*/,   int /*max_i*/,   int /*max_j*/,  
     244   const geometry_msgs::msg::Pose2D & pose )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           if (  !filter_mask_ ) {
           // Show warning message every 2 seconds to not litter an output
           RCLCPP_WARN_THROTTLE(  
           logger_,   *(  clock_ ),   2000,  
           "SpeedFilter: Filter mask was not received" );
           return;
           }
          
           geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame
          
           // Transforming robot pose from current layer frame to mask frame
           if (  !transformPose(  pose,   mask_pose ) ) {
           return;
           }
          
           // Converting mask_pose robot position to filter_mask_ indexes (  mask_robot_i,   mask_robot_j )
           unsigned int mask_robot_i,   mask_robot_j;
           if (  !worldToMask(  mask_pose.x,   mask_pose.y,   mask_robot_i,   mask_robot_j ) ) {
           return;
           }
          
           // Getting filter_mask data from cell where the robot placed and
           // calculating speed limit value
           int8_t speed_mask_data = getMaskData(  mask_robot_i,   mask_robot_j );
           if (  speed_mask_data == SPEED_MASK_NO_LIMIT ) {
           // Corresponding filter mask cell is free.
           // Setting no speed limit there.
           speed_limit_ = NO_SPEED_LIMIT;
           } else if (  speed_mask_data == SPEED_MASK_UNKNOWN ) {
           // Corresponding filter mask cell is unknown.
           // Do nothing.
           RCLCPP_ERROR(  
           logger_,  
           "SpeedFilter: Found unknown cell in filter_mask[%i,   %i],   "
           "which is invalid for this kind of filter",  
           mask_robot_i,   mask_robot_j );
           return;
           } else {
           // Normal case: speed_mask_data in range of [1..100]
           speed_limit_ = speed_mask_data * multiplier_ + base_;
           if (  percentage_ ) {
           if (  speed_limit_ < 0.0 || speed_limit_ > 100.0 ) {
           RCLCPP_WARN(  
           logger_,  
           "SpeedFilter: Speed limit in filter_mask[%i,   %i] is %f%%,   "
           "out of bounds of [0,   100]. Setting it to no-limit value.",  
           mask_robot_i,   mask_robot_j,   speed_limit_ );
           speed_limit_ = NO_SPEED_LIMIT;
           }
           } else {
           if (  speed_limit_ < 0.0 ) {
           RCLCPP_WARN(  
           logger_,  
           "SpeedFilter: Speed limit in filter_mask[%i,   %i] is less than 0 m/s,   "
           "which can not be true. Setting it to no-limit value.",  
           mask_robot_i,   mask_robot_j );
           speed_limit_ = NO_SPEED_LIMIT;
           }
           }
           }
          
           if (  speed_limit_ != speed_limit_prev_ ) {
           if (  speed_limit_ != NO_SPEED_LIMIT ) {
           RCLCPP_DEBUG(  logger_,   "SpeedFilter: Speed limit is set to %f",   speed_limit_ );
           } else {
           RCLCPP_DEBUG(  logger_,   "SpeedFilter: Speed limit is set to its default value" );
           }
          
           // Forming and publishing new SpeedLimit message
           std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
           std::make_unique<nav2_msgs::msg::SpeedLimit>(   );
           msg->header.frame_id = global_frame_;
           msg->header.stamp = clock_->now(   );
           msg->percentage = percentage_;
           msg->speed_limit = speed_limit_;
           speed_limit_pub_->publish(  std::move(  msg ) );
          
           speed_limit_prev_ = speed_limit_;
           }
          }
          
     329  void SpeedFilter::resetFilter(   )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           filter_info_sub_.reset(   );
           mask_sub_.reset(   );
           if (  speed_limit_pub_ ) {
           speed_limit_pub_->on_deactivate(   );
           speed_limit_pub_.reset(   );
           }
          }
          
     341  bool SpeedFilter::isActive(   )
          {
           std::lock_guard<CostmapFilter::mutex_t> guard(  *getMutex(   ) );
          
           if (  filter_mask_ ) {
           return true;
           }
           return false;
          }
          
          } // namespace nav2_costmap_2d
          
          #include "pluginlib/class_list_macros.hpp"
     354  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::SpeedFilter,   nav2_costmap_2d::Layer )

navigation2/nav2_costmap_2d/plugins/inflation_layer.cpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #include "nav2_costmap_2d/inflation_layer.hpp"
          
          #include <limits>
          #include <map>
          #include <vector>
          #include <algorithm>
          #include <utility>
          
          #include "nav2_costmap_2d/costmap_math.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "rclcpp/parameter_events_filter.hpp"
          
      51  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::InflationLayer,   nav2_costmap_2d::Layer )
          
          using nav2_costmap_2d::LETHAL_OBSTACLE;
          using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
          using nav2_costmap_2d::NO_INFORMATION;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_costmap_2d
          {
          
      61  InflationLayer::InflationLayer(   )
          : inflation_radius_(  0 ),  
           inscribed_radius_(  0 ),  
           cost_scaling_factor_(  0 ),  
           inflate_unknown_(  false ),  
           inflate_around_unknown_(  false ),  
           cell_inflation_radius_(  0 ),  
           cached_cell_inflation_radius_(  0 ),  
           resolution_(  0 ),  
           cache_length_(  0 ),  
           last_min_x_(  std::numeric_limits<double>::lowest(   ) ),  
           last_min_y_(  std::numeric_limits<double>::lowest(   ) ),  
           last_max_x_(  std::numeric_limits<double>::max(   ) ),  
           last_max_y_(  std::numeric_limits<double>::max(   ) )
          {
           access_ = new mutex_t(   );
          }
          
          InflationLayer::~InflationLayer(   )
          {
           dyn_params_handler_.reset(   );
           delete access_;
          }
          
          void
          InflationLayer::onInitialize(   )
          {
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "inflation_radius",   rclcpp::ParameterValue(  0.55 ) );
           declareParameter(  "cost_scaling_factor",   rclcpp::ParameterValue(  10.0 ) );
           declareParameter(  "inflate_unknown",   rclcpp::ParameterValue(  false ) );
           declareParameter(  "inflate_around_unknown",   rclcpp::ParameterValue(  false ) );
          
           {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           node->get_parameter(  name_ + "." + "inflation_radius",   inflation_radius_ );
           node->get_parameter(  name_ + "." + "cost_scaling_factor",   cost_scaling_factor_ );
           node->get_parameter(  name_ + "." + "inflate_unknown",   inflate_unknown_ );
           node->get_parameter(  name_ + "." + "inflate_around_unknown",   inflate_around_unknown_ );
          
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &InflationLayer::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
           }
          
           current_ = true;
           seen_.clear(   );
           cached_distances_.clear(   );
           cached_costs_.clear(   );
           need_reinflation_ = false;
           cell_inflation_radius_ = cellDistance(  inflation_radius_ );
           matchSize(   );
          }
          
          void
          InflationLayer::matchSize(   )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap(   );
           resolution_ = costmap->getResolution(   );
           cell_inflation_radius_ = cellDistance(  inflation_radius_ );
           computeCaches(   );
           seen_ = std::vector<bool>(  costmap->getSizeInCellsX(   ) * costmap->getSizeInCellsY(   ),   false );
          }
          
          void
          InflationLayer::updateBounds(  
           double /*robot_x*/,   double /*robot_y*/,   double /*robot_yaw*/,   double * min_x,  
           double * min_y,   double * max_x,   double * max_y )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  need_reinflation_ ) {
           last_min_x_ = *min_x;
           last_min_y_ = *min_y;
           last_max_x_ = *max_x;
           last_max_y_ = *max_y;
          
           *min_x = std::numeric_limits<double>::lowest(   );
           *min_y = std::numeric_limits<double>::lowest(   );
           *max_x = std::numeric_limits<double>::max(   );
           *max_y = std::numeric_limits<double>::max(   );
           need_reinflation_ = false;
           } else {
           double tmp_min_x = last_min_x_;
           double tmp_min_y = last_min_y_;
           double tmp_max_x = last_max_x_;
           double tmp_max_y = last_max_y_;
           last_min_x_ = *min_x;
           last_min_y_ = *min_y;
           last_max_x_ = *max_x;
           last_max_y_ = *max_y;
           *min_x = std::min(  tmp_min_x,   *min_x ) - inflation_radius_;
           *min_y = std::min(  tmp_min_y,   *min_y ) - inflation_radius_;
           *max_x = std::max(  tmp_max_x,   *max_x ) + inflation_radius_;
           *max_y = std::max(  tmp_max_y,   *max_y ) + inflation_radius_;
           }
          }
          
          void
          InflationLayer::onFootprintChanged(   )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           inscribed_radius_ = layered_costmap_->getInscribedRadius(   );
           cell_inflation_radius_ = cellDistance(  inflation_radius_ );
           computeCaches(   );
           need_reinflation_ = true;
          
           RCLCPP_DEBUG(  
           logger_,   "InflationLayer::onFootprintChanged(   ): num footprint points: %zu,  "
           " inscribed_radius_ = %.3f,   inflation_radius_ = %.3f",  
           layered_costmap_->getFootprint(   ).size(   ),   inscribed_radius_,   inflation_radius_ );
          }
          
          void
          InflationLayer::updateCosts(  
           nav2_costmap_2d::Costmap2D & master_grid,   int min_i,   int min_j,  
           int max_i,  
           int max_j )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  !enabled_ || (  cell_inflation_radius_ == 0 ) ) {
           return;
           }
          
           // make sure the inflation list is empty at the beginning of the cycle (  should always be true )
           for (  auto & dist : inflation_cells_ ) {
           RCLCPP_FATAL_EXPRESSION(  
           logger_,  
           !dist.empty(   ),   "The inflation list must be empty at the beginning of inflation" );
           }
          
           unsigned char * master_array = master_grid.getCharMap(   );
           unsigned int size_x = master_grid.getSizeInCellsX(   ),   size_y = master_grid.getSizeInCellsY(   );
          
           if (  seen_.size(   ) != size_x * size_y ) {
           RCLCPP_WARN(  
           logger_,   "InflationLayer::updateCosts(   ): seen_ vector size is wrong" );
           seen_ = std::vector<bool>(  size_x * size_y,   false );
           }
          
           std::fill(  begin(  seen_ ),   end(  seen_ ),   false );
          
           // We need to include in the inflation cells outside the bounding
           // box min_i...max_j,   by the amount cell_inflation_radius_. Cells
           // up to that distance outside the box can still influence the costs
           // stored in cells inside the box.
           const int base_min_i = min_i;
           const int base_min_j = min_j;
           const int base_max_i = max_i;
           const int base_max_j = max_j;
           min_i -= static_cast<int>(  cell_inflation_radius_ );
           min_j -= static_cast<int>(  cell_inflation_radius_ );
           max_i += static_cast<int>(  cell_inflation_radius_ );
           max_j += static_cast<int>(  cell_inflation_radius_ );
          
           min_i = std::max(  0,   min_i );
           min_j = std::max(  0,   min_j );
           max_i = std::min(  static_cast<int>(  size_x ),   max_i );
           max_j = std::min(  static_cast<int>(  size_y ),   max_j );
          
           // Inflation list; we append cells to visit in a list associated with
           // its distance to the nearest obstacle
           // We use a map<distance,   list> to emulate the priority queue used before,  
           // with a notable performance boost
          
           // Start with lethal obstacles: by definition distance is 0.0
           auto & obs_bin = inflation_cells_[0];
           for (  int j = min_j; j < max_j; j++ ) {
           for (  int i = min_i; i < max_i; i++ ) {
           int index = static_cast<int>(  master_grid.getIndex(  i,   j ) );
           unsigned char cost = master_array[index];
           if (  cost == LETHAL_OBSTACLE || (  inflate_around_unknown_ && cost == NO_INFORMATION ) ) {
           obs_bin.emplace_back(  index,   i,   j,   i,   j );
           }
           }
           }
          
           // Process cells by increasing distance; new cells are appended to the
           // corresponding distance bin,   so they
           // can overtake previously inserted but farther away cells
           for (  const auto & dist_bin : inflation_cells_ ) {
           for (  std::size_t i = 0; i < dist_bin.size(   ); ++i ) {
           // Do not use iterator or for-range based loops to
           // iterate though dist_bin,   since it's size might
           // change when a new cell is enqueued,   invalidating all iterators
           unsigned int index = dist_bin[i].index_;
          
           // ignore if already visited
           if (  seen_[index] ) {
           continue;
           }
          
           seen_[index] = true;
          
           unsigned int mx = dist_bin[i].x_;
           unsigned int my = dist_bin[i].y_;
           unsigned int sx = dist_bin[i].src_x_;
           unsigned int sy = dist_bin[i].src_y_;
          
           // assign the cost associated with the distance from an obstacle to the cell
           unsigned char cost = costLookup(  mx,   my,   sx,   sy );
           unsigned char old_cost = master_array[index];
           // In order to avoid artifacts appeared out of boundary areas
           // when some layer is going after inflation_layer,  
           // we need to apply inflation_layer only to inside of given bounds
           if (  static_cast<int>(  mx ) >= base_min_i &&
           static_cast<int>(  my ) >= base_min_j &&
           static_cast<int>(  mx ) < base_max_i &&
           static_cast<int>(  my ) < base_max_j )
           {
           if (  old_cost == NO_INFORMATION &&
           (  inflate_unknown_ ? (  cost > FREE_SPACE ) : (  cost >= INSCRIBED_INFLATED_OBSTACLE ) ) )
           {
           master_array[index] = cost;
           } else {
           master_array[index] = std::max(  old_cost,   cost );
           }
           }
          
           // attempt to put the neighbors of the current cell onto the inflation list
           if (  mx > 0 ) {
           enqueue(  index - 1,   mx - 1,   my,   sx,   sy );
           }
           if (  my > 0 ) {
           enqueue(  index - size_x,   mx,   my - 1,   sx,   sy );
           }
           if (  mx < size_x - 1 ) {
           enqueue(  index + 1,   mx + 1,   my,   sx,   sy );
           }
           if (  my < size_y - 1 ) {
           enqueue(  index + size_x,   mx,   my + 1,   sx,   sy );
           }
           }
           }
          
           for (  auto & dist : inflation_cells_ ) {
           dist.clear(   );
           dist.reserve(  200 );
           }
          
           current_ = true;
          }
          
          /**
           * @brief Given an index of a cell in the costmap,   place it into a list pending for obstacle inflation
           * @param grid The costmap
           * @param index The index of the cell
           * @param mx The x coordinate of the cell (  can be computed from the index,   but saves time to store it )
           * @param my The y coordinate of the cell (  can be computed from the index,   but saves time to store it )
           * @param src_x The x index of the obstacle point inflation started at
           * @param src_y The y index of the obstacle point inflation started at
           */
          void
          InflationLayer::enqueue(  
           unsigned int index,   unsigned int mx,   unsigned int my,  
           unsigned int src_x,   unsigned int src_y )
          {
           if (  !seen_[index] ) {
           // we compute our distance table one cell further than the
           // inflation radius dictates so we can make the check below
           double distance = distanceLookup(  mx,   my,   src_x,   src_y );
          
           // we only want to put the cell in the list if it is within
           // the inflation radius of the obstacle point
           if (  distance > cell_inflation_radius_ ) {
           return;
           }
          
           const unsigned int r = cell_inflation_radius_ + 2;
          
           // push the cell data onto the inflation list and mark
           inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(  
           index,   mx,   my,   src_x,   src_y );
           }
          }
          
          void
          InflationLayer::computeCaches(   )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  cell_inflation_radius_ == 0 ) {
           return;
           }
          
           cache_length_ = cell_inflation_radius_ + 2;
          
           // based on the inflation radius... compute distance and cost caches
           if (  cell_inflation_radius_ != cached_cell_inflation_radius_ ) {
           cached_costs_.resize(  cache_length_ * cache_length_ );
           cached_distances_.resize(  cache_length_ * cache_length_ );
          
           for (  unsigned int i = 0; i < cache_length_; ++i ) {
           for (  unsigned int j = 0; j < cache_length_; ++j ) {
           cached_distances_[i * cache_length_ + j] = hypot(  i,   j );
           }
           }
          
           cached_cell_inflation_radius_ = cell_inflation_radius_;
           }
          
           for (  unsigned int i = 0; i < cache_length_; ++i ) {
           for (  unsigned int j = 0; j < cache_length_; ++j ) {
           cached_costs_[i * cache_length_ + j] = computeCost(  cached_distances_[i * cache_length_ + j] );
           }
           }
          
           int max_dist = generateIntegerDistances(   );
           inflation_cells_.clear(   );
           inflation_cells_.resize(  max_dist + 1 );
           for (  auto & dist : inflation_cells_ ) {
           dist.reserve(  200 );
           }
          }
          
          int
          InflationLayer::generateIntegerDistances(   )
          {
           const int r = cell_inflation_radius_ + 2;
           const int size = r * 2 + 1;
          
           std::vector<std::pair<int,   int>> points;
          
           for (  int y = -r; y <= r; y++ ) {
           for (  int x = -r; x <= r; x++ ) {
           if (  x * x + y * y <= r * r ) {
           points.emplace_back(  x,   y );
           }
           }
           }
          
           std::sort(  
           points.begin(   ),   points.end(   ),  
           [](  const std::pair<int,   int> & a,   const std::pair<int,   int> & b ) -> bool {
           return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second;
           }
            );
          
           std::vector<std::vector<int>> distance_matrix(  size,   std::vector<int>(  size,   0 ) );
           std::pair<int,   int> last = {0,   0};
           int level = 0;
           for (  auto const & p : points ) {
           if (  p.first * p.first + p.second * p.second !=
           last.first * last.first + last.second * last.second )
           {
           level++;
           }
           distance_matrix[p.first + r][p.second + r] = level;
           last = p;
           }
          
           distance_matrix_ = distance_matrix;
           return level;
          }
          
          /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
          rcl_interfaces::msg::SetParametersResult
          InflationLayer::dynamicParametersCallback(  
           std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           rcl_interfaces::msg::SetParametersResult result;
          
           bool need_cache_recompute = false;
          
           for (  auto parameter : parameters ) {
           const auto & param_type = parameter.get_type(   );
           const auto & param_name = parameter.get_name(   );
          
           if (  param_type == ParameterType::PARAMETER_DOUBLE ) {
           if (  param_name == name_ + "." + "inflation_radius" &&
           inflation_radius_ != parameter.as_double(   ) )
           {
           inflation_radius_ = parameter.as_double(   );
           need_reinflation_ = true;
           need_cache_recompute = true;
           } else if (  param_name == name_ + "." + "cost_scaling_factor" && // NOLINT
           cost_scaling_factor_ != parameter.as_double(   ) )
           {
           cost_scaling_factor_ = parameter.as_double(   );
           need_reinflation_ = true;
           need_cache_recompute = true;
           }
           } else if (  param_type == ParameterType::PARAMETER_BOOL ) {
           if (  param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool(   ) ) {
           enabled_ = parameter.as_bool(   );
           need_reinflation_ = true;
           current_ = false;
           } else if (  param_name == name_ + "." + "inflate_unknown" && // NOLINT
           inflate_unknown_ != parameter.as_bool(   ) )
           {
           inflate_unknown_ = parameter.as_bool(   );
           need_reinflation_ = true;
           } else if (  param_name == name_ + "." + "inflate_around_unknown" && // NOLINT
           inflate_around_unknown_ != parameter.as_bool(   ) )
           {
           inflate_around_unknown_ = parameter.as_bool(   );
           need_reinflation_ = true;
           }
           }
           }
          
           if (  need_cache_recompute ) {
           matchSize(   );
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           * Steve Macenski
           *********************************************************************/
          #include "nav2_costmap_2d/obstacle_layer.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "pluginlib/class_list_macros.hpp"
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          #include "nav2_costmap_2d/costmap_math.hpp"
          
      50  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::ObstacleLayer,   nav2_costmap_2d::Layer )
          
          using nav2_costmap_2d::NO_INFORMATION;
          using nav2_costmap_2d::LETHAL_OBSTACLE;
          using nav2_costmap_2d::FREE_SPACE;
          
          using nav2_costmap_2d::ObservationBuffer;
          using nav2_costmap_2d::Observation;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_costmap_2d
          {
          
      63  ObstacleLayer::~ObstacleLayer(   )
          {
           dyn_params_handler_.reset(   );
           for (  auto & notifier : observation_notifiers_ ) {
           notifier.reset(   );
           }
          }
          
      71  void ObstacleLayer::onInitialize(   )
          {
           bool track_unknown_space;
           double transform_tolerance;
          
           // The topics that we'll subscribe to from the parameter server
           std::string topics_string;
          
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "footprint_clearing_enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "max_obstacle_height",   rclcpp::ParameterValue(  2.0 ) );
           declareParameter(  "combination_method",   rclcpp::ParameterValue(  1 ) );
           declareParameter(  "observation_sources",   rclcpp::ParameterValue(  std::string(  "" ) ) );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           node->get_parameter(  name_ + "." + "footprint_clearing_enabled",   footprint_clearing_enabled_ );
           node->get_parameter(  name_ + "." + "max_obstacle_height",   max_obstacle_height_ );
           node->get_parameter(  name_ + "." + "combination_method",   combination_method_ );
           node->get_parameter(  "track_unknown_space",   track_unknown_space );
           node->get_parameter(  "transform_tolerance",   transform_tolerance );
           node->get_parameter(  name_ + "." + "observation_sources",   topics_string );
          
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &ObstacleLayer::dynamicParametersCallback,  
           this,  
           std::placeholders::_1 ) );
          
           RCLCPP_INFO(  
           logger_,  
           "Subscribed to Topics: %s",   topics_string.c_str(   ) );
          
           rolling_window_ = layered_costmap_->isRolling(   );
          
           if (  track_unknown_space ) {
           default_value_ = NO_INFORMATION;
           } else {
           default_value_ = FREE_SPACE;
           }
          
           ObstacleLayer::matchSize(   );
           current_ = true;
           was_reset_ = false;
          
           global_frame_ = layered_costmap_->getGlobalFrameID(   );
          
           auto sub_opt = rclcpp::SubscriptionOptions(   );
           sub_opt.callback_group = callback_group_;
          
           // now we need to split the topics based on whitespace which we can use a stringstream for
           std::stringstream ss(  topics_string );
          
           std::string source;
           while (  ss >> source ) {
           // get the parameters for the specific topic
           double observation_keep_time,   expected_update_rate,   min_obstacle_height,   max_obstacle_height;
           std::string topic,   sensor_frame,   data_type;
           bool inf_is_valid,   clearing,   marking;
          
           declareParameter(  source + "." + "topic",   rclcpp::ParameterValue(  source ) );
           declareParameter(  source + "." + "sensor_frame",   rclcpp::ParameterValue(  std::string(  "" ) ) );
           declareParameter(  source + "." + "observation_persistence",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  source + "." + "expected_update_rate",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  source + "." + "data_type",   rclcpp::ParameterValue(  std::string(  "LaserScan" ) ) );
           declareParameter(  source + "." + "min_obstacle_height",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  source + "." + "max_obstacle_height",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  source + "." + "inf_is_valid",   rclcpp::ParameterValue(  false ) );
           declareParameter(  source + "." + "marking",   rclcpp::ParameterValue(  true ) );
           declareParameter(  source + "." + "clearing",   rclcpp::ParameterValue(  false ) );
           declareParameter(  source + "." + "obstacle_max_range",   rclcpp::ParameterValue(  2.5 ) );
           declareParameter(  source + "." + "obstacle_min_range",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  source + "." + "raytrace_max_range",   rclcpp::ParameterValue(  3.0 ) );
           declareParameter(  source + "." + "raytrace_min_range",   rclcpp::ParameterValue(  0.0 ) );
          
           node->get_parameter(  name_ + "." + source + "." + "topic",   topic );
           node->get_parameter(  name_ + "." + source + "." + "sensor_frame",   sensor_frame );
           node->get_parameter(  
           name_ + "." + source + "." + "observation_persistence",  
           observation_keep_time );
           node->get_parameter(  
           name_ + "." + source + "." + "expected_update_rate",  
           expected_update_rate );
           node->get_parameter(  name_ + "." + source + "." + "data_type",   data_type );
           node->get_parameter(  name_ + "." + source + "." + "min_obstacle_height",   min_obstacle_height );
           node->get_parameter(  name_ + "." + source + "." + "max_obstacle_height",   max_obstacle_height );
           node->get_parameter(  name_ + "." + source + "." + "inf_is_valid",   inf_is_valid );
           node->get_parameter(  name_ + "." + source + "." + "marking",   marking );
           node->get_parameter(  name_ + "." + source + "." + "clearing",   clearing );
          
           if (  !(  data_type == "PointCloud2" || data_type == "LaserScan" ) ) {
           RCLCPP_FATAL(  
           logger_,  
           "Only topics that use point cloud2s or laser scans are currently supported" );
           throw std::runtime_error(  
           "Only topics that use point cloud2s or laser scans are currently supported" );
           }
          
           // get the obstacle range for the sensor
           double obstacle_max_range,   obstacle_min_range;
           node->get_parameter(  name_ + "." + source + "." + "obstacle_max_range",   obstacle_max_range );
           node->get_parameter(  name_ + "." + source + "." + "obstacle_min_range",   obstacle_min_range );
          
           // get the raytrace ranges for the sensor
           double raytrace_max_range,   raytrace_min_range;
           node->get_parameter(  name_ + "." + source + "." + "raytrace_min_range",   raytrace_min_range );
           node->get_parameter(  name_ + "." + source + "." + "raytrace_max_range",   raytrace_max_range );
          
          
           RCLCPP_DEBUG(  
           logger_,  
           "Creating an observation buffer for source %s,   topic %s,   frame %s",  
           source.c_str(   ),   topic.c_str(   ),  
           sensor_frame.c_str(   ) );
          
           // create an observation buffer
           observation_buffers_.push_back(  
           std::shared_ptr<ObservationBuffer
           >(  
           new ObservationBuffer(  
           node,   topic,   observation_keep_time,   expected_update_rate,  
           min_obstacle_height,  
           max_obstacle_height,   obstacle_max_range,   obstacle_min_range,   raytrace_max_range,  
           raytrace_min_range,   *tf_,  
           global_frame_,  
           sensor_frame,   tf2::durationFromSec(  transform_tolerance ) ) ) );
          
           // check if we'll add this buffer to our marking observation buffers
           if (  marking ) {
           marking_buffers_.push_back(  observation_buffers_.back(   ) );
           }
          
           // check if we'll also add this buffer to our clearing observation buffers
           if (  clearing ) {
           clearing_buffers_.push_back(  observation_buffers_.back(   ) );
           }
          
           RCLCPP_DEBUG(  
           logger_,  
           "Created an observation buffer for source %s,   topic %s,   global frame: %s,   "
           "expected update rate: %.2f,   observation persistence: %.2f",  
           source.c_str(   ),   topic.c_str(   ),  
           global_frame_.c_str(   ),   expected_update_rate,   observation_keep_time );
          
           rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
           custom_qos_profile.depth = 50;
          
           // create a callback for the topic
           if (  data_type == "LaserScan" ) {
           auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,  
           rclcpp_lifecycle::LifecycleNode>>(  node,   topic,   custom_qos_profile,   sub_opt );
           sub->unsubscribe(   );
          
           auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(  
           *sub,   *tf_,   global_frame_,   50,  
           node->get_node_logging_interface(   ),  
           node->get_node_clock_interface(   ),  
           tf2::durationFromSec(  transform_tolerance ) );
          
           if (  inf_is_valid ) {
           filter->registerCallback(  
           std::bind(  
           &ObstacleLayer::laserScanValidInfCallback,   this,   std::placeholders::_1,  
           observation_buffers_.back(   ) ) );
          
           } else {
           filter->registerCallback(  
           std::bind(  
           &ObstacleLayer::laserScanCallback,   this,   std::placeholders::_1,  
           observation_buffers_.back(   ) ) );
           }
          
           observation_subscribers_.push_back(  sub );
          
           observation_notifiers_.push_back(  filter );
           observation_notifiers_.back(   )->setTolerance(  rclcpp::Duration::from_seconds(  0.05 ) );
          
           } else {
           auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,  
           rclcpp_lifecycle::LifecycleNode>>(  node,   topic,   custom_qos_profile,   sub_opt );
           sub->unsubscribe(   );
          
           if (  inf_is_valid ) {
           RCLCPP_WARN(  
           logger_,  
           "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations." );
           }
          
           auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(  
           *sub,   *tf_,   global_frame_,   50,  
           node->get_node_logging_interface(   ),  
           node->get_node_clock_interface(   ),  
           tf2::durationFromSec(  transform_tolerance ) );
          
           filter->registerCallback(  
           std::bind(  
           &ObstacleLayer::pointCloud2Callback,   this,   std::placeholders::_1,  
           observation_buffers_.back(   ) ) );
          
           observation_subscribers_.push_back(  sub );
           observation_notifiers_.push_back(  filter );
           }
          
           if (  sensor_frame != "" ) {
           std::vector<std::string> target_frames;
           target_frames.push_back(  global_frame_ );
           target_frames.push_back(  sensor_frame );
           observation_notifiers_.back(   )->setTargetFrames(  target_frames );
           }
           }
          }
          
          rcl_interfaces::msg::SetParametersResult
     288  ObstacleLayer::dynamicParametersCallback(  
     289   std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           rcl_interfaces::msg::SetParametersResult result;
          
           for (  auto parameter : parameters ) {
           const auto & param_type = parameter.get_type(   );
           const auto & param_name = parameter.get_name(   );
          
           if (  param_type == ParameterType::PARAMETER_DOUBLE ) {
           if (  param_name == name_ + "." + "max_obstacle_height" ) {
           max_obstacle_height_ = parameter.as_double(   );
           }
           } else if (  param_type == ParameterType::PARAMETER_BOOL ) {
           if (  param_name == name_ + "." + "enabled" ) {
           enabled_ = parameter.as_bool(   );
           } else if (  param_name == name_ + "." + "footprint_clearing_enabled" ) {
           footprint_clearing_enabled_ = parameter.as_bool(   );
           }
           } else if (  param_type == ParameterType::PARAMETER_INTEGER ) {
           if (  param_name == name_ + "." + "combination_method" ) {
           combination_method_ = parameter.as_int(   );
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          void
     320  ObstacleLayer::laserScanCallback(  
     321   sensor_msgs::msg::LaserScan::ConstSharedPtr message,  
     322   const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer )
          {
           // project the laser into a point cloud
           sensor_msgs::msg::PointCloud2 cloud;
           cloud.header = message->header;
          
           // project the scan into a point cloud
           try {
           projector_.transformLaserScanToPointCloud(  message->header.frame_id,   *message,   cloud,   *tf_ );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_WARN(  
           logger_,  
           "High fidelity enabled,   but TF returned a transform exception to frame %s: %s",  
           global_frame_.c_str(   ),  
           ex.what(   ) );
           projector_.projectLaser(  *message,   cloud );
           } catch (  std::runtime_error & ex ) {
           RCLCPP_WARN(  
           logger_,  
           "transformLaserScanToPointCloud error,   it seems the message from laser is malformed."
           " Ignore this message. what(   ): %s",  
           ex.what(   ) );
           return;
           }
          
           // buffer the point cloud
           buffer->lock(   );
           buffer->bufferCloud(  cloud );
           buffer->unlock(   );
          }
          
          void
     354  ObstacleLayer::laserScanValidInfCallback(  
     355   sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message,  
     356   const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer )
          {
           // Filter positive infinities (  "Inf"s ) to max_range.
           float epsilon = 0.0001; // a tenth of a millimeter
           sensor_msgs::msg::LaserScan message = *raw_message;
           for (  size_t i = 0; i < message.ranges.size(   ); i++ ) {
           float range = message.ranges[i];
           if (  !std::isfinite(  range ) && range > 0 ) {
           message.ranges[i] = message.range_max - epsilon;
           }
           }
          
           // project the laser into a point cloud
           sensor_msgs::msg::PointCloud2 cloud;
           cloud.header = message.header;
          
           // project the scan into a point cloud
           try {
           projector_.transformLaserScanToPointCloud(  message.header.frame_id,   message,   cloud,   *tf_ );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_WARN(  
           logger_,  
           "High fidelity enabled,   but TF returned a transform exception to frame %s: %s",  
           global_frame_.c_str(   ),   ex.what(   ) );
           projector_.projectLaser(  message,   cloud );
           } catch (  std::runtime_error & ex ) {
           RCLCPP_WARN(  
           logger_,  
           "transformLaserScanToPointCloud error,   it seems the message from laser is malformed."
           " Ignore this message. what(   ): %s",  
           ex.what(   ) );
           return;
           }
          
           // buffer the point cloud
           buffer->lock(   );
           buffer->bufferCloud(  cloud );
           buffer->unlock(   );
          }
          
          void
     397  ObstacleLayer::pointCloud2Callback(  
     398   sensor_msgs::msg::PointCloud2::ConstSharedPtr message,  
     399   const std::shared_ptr<ObservationBuffer> & buffer )
          {
           // buffer the point cloud
           buffer->lock(   );
           buffer->bufferCloud(  *message );
           buffer->unlock(   );
          }
          
          void
     408  ObstacleLayer::updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,   double * max_x,   double * max_y )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  rolling_window_ ) {
           updateOrigin(  robot_x - getSizeInMetersX(   ) / 2,   robot_y - getSizeInMetersY(   ) / 2 );
           }
           if (  !enabled_ ) {
           return;
           }
           useExtraBounds(  min_x,   min_y,   max_x,   max_y );
          
           bool current = true;
           std::vector<Observation> observations,   clearing_observations;
          
           // get the marking observations
           current = current && getMarkingObservations(  observations );
          
           // get the clearing observations
           current = current && getClearingObservations(  clearing_observations );
          
           // update the global current status
           current_ = current;
          
           // raytrace freespace
           for (  unsigned int i = 0; i < clearing_observations.size(   ); ++i ) {
           raytraceFreespace(  clearing_observations[i],   min_x,   min_y,   max_x,   max_y );
           }
          
           // place the new obstacles into a priority queue... each with a priority of zero to begin with
           for (  std::vector<Observation>::const_iterator it = observations.begin(   );
           it != observations.end(   ); ++it )
           {
           const Observation & obs = *it;
          
           const sensor_msgs::msg::PointCloud2 & cloud = *(  obs.cloud_ );
          
           double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
           double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
          
           sensor_msgs::PointCloud2ConstIterator<float> iter_x(  cloud,   "x" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_y(  cloud,   "y" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_z(  cloud,   "z" );
          
           for (  ; iter_x != iter_x.end(   ); ++iter_x,   ++iter_y,   ++iter_z ) {
           double px = *iter_x,   py = *iter_y,   pz = *iter_z;
          
           // if the obstacle is too high or too far away from the robot we won't add it
           if (  pz > max_obstacle_height_ ) {
           RCLCPP_DEBUG(  logger_,   "The point is too high" );
           continue;
           }
          
           // compute the squared distance from the hitpoint to the pointcloud's origin
           double sq_dist =
           (  px -
           obs.origin_.x ) * (  px - obs.origin_.x ) + (  py - obs.origin_.y ) * (  py - obs.origin_.y ) +
           (  pz - obs.origin_.z ) * (  pz - obs.origin_.z );
          
           // if the point is far enough away... we won't consider it
           if (  sq_dist >= sq_obstacle_max_range ) {
           RCLCPP_DEBUG(  logger_,   "The point is too far away" );
           continue;
           }
          
           // if the point is too close,   do not conisder it
           if (  sq_dist < sq_obstacle_min_range ) {
           RCLCPP_DEBUG(  logger_,   "The point is too close" );
           continue;
           }
          
           // now we need to compute the map coordinates for the observation
           unsigned int mx,   my;
           if (  !worldToMap(  px,   py,   mx,   my ) ) {
           RCLCPP_DEBUG(  logger_,   "Computing map coords failed" );
           continue;
           }
          
           unsigned int index = getIndex(  mx,   my );
           costmap_[index] = LETHAL_OBSTACLE;
           touch(  px,   py,   min_x,   min_y,   max_x,   max_y );
           }
           }
          
           updateFootprint(  robot_x,   robot_y,   robot_yaw,   min_x,   min_y,   max_x,   max_y );
          }
          
          void
     497  ObstacleLayer::updateFootprint(  
           double robot_x,   double robot_y,   double robot_yaw,  
           double * min_x,   double * min_y,  
           double * max_x,  
           double * max_y )
          {
           if (  !footprint_clearing_enabled_ ) {return;}
           transformFootprint(  robot_x,   robot_y,   robot_yaw,   getFootprint(   ),   transformed_footprint_ );
          
           for (  unsigned int i = 0; i < transformed_footprint_.size(   ); i++ ) {
           touch(  transformed_footprint_[i].x,   transformed_footprint_[i].y,   min_x,   min_y,   max_x,   max_y );
           }
          }
          
          void
     512  ObstacleLayer::updateCosts(  
     513   nav2_costmap_2d::Costmap2D & master_grid,   int min_i,   int min_j,  
           int max_i,  
           int max_j )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  !enabled_ ) {
           return;
           }
          
           // if not current due to reset,   set current now after clearing
           if (  !current_ && was_reset_ ) {
           was_reset_ = false;
           current_ = true;
           }
          
           if (  footprint_clearing_enabled_ ) {
           setConvexPolygonCost(  transformed_footprint_,   nav2_costmap_2d::FREE_SPACE );
           }
          
           switch (  combination_method_ ) {
           case 0: // Overwrite
           updateWithOverwrite(  master_grid,   min_i,   min_j,   max_i,   max_j );
           break;
           case 1: // Maximum
           updateWithMax(  master_grid,   min_i,   min_j,   max_i,   max_j );
           break;
           default: // Nothing
           break;
           }
          }
          
          void
     545  ObstacleLayer::addStaticObservation(  
     546   nav2_costmap_2d::Observation & obs,  
     547   bool marking,   bool clearing )
          {
           if (  marking ) {
           static_marking_observations_.push_back(  obs );
           }
           if (  clearing ) {
           static_clearing_observations_.push_back(  obs );
           }
          }
          
          void
     558  ObstacleLayer::clearStaticObservations(  bool marking,   bool clearing )
          {
           if (  marking ) {
           static_marking_observations_.clear(   );
           }
           if (  clearing ) {
           static_clearing_observations_.clear(   );
           }
          }
          
          bool
     569  ObstacleLayer::getMarkingObservations(  std::vector<Observation> & marking_observations ) const
          {
           bool current = true;
           // get the marking observations
           for (  unsigned int i = 0; i < marking_buffers_.size(   ); ++i ) {
           marking_buffers_[i]->lock(   );
           marking_buffers_[i]->getObservations(  marking_observations );
           current = marking_buffers_[i]->isCurrent(   ) && current;
           marking_buffers_[i]->unlock(   );
           }
           marking_observations.insert(  
           marking_observations.end(   ),  
           static_marking_observations_.begin(   ),   static_marking_observations_.end(   ) );
           return current;
          }
          
          bool
     586  ObstacleLayer::getClearingObservations(  std::vector<Observation> & clearing_observations ) const
          {
           bool current = true;
           // get the clearing observations
           for (  unsigned int i = 0; i < clearing_buffers_.size(   ); ++i ) {
           clearing_buffers_[i]->lock(   );
           clearing_buffers_[i]->getObservations(  clearing_observations );
           current = clearing_buffers_[i]->isCurrent(   ) && current;
           clearing_buffers_[i]->unlock(   );
           }
           clearing_observations.insert(  
           clearing_observations.end(   ),  
           static_clearing_observations_.begin(   ),   static_clearing_observations_.end(   ) );
           return current;
          }
          
          void
     603  ObstacleLayer::raytraceFreespace(  
     604   const Observation & clearing_observation,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y )
          {
           double ox = clearing_observation.origin_.x;
           double oy = clearing_observation.origin_.y;
           const sensor_msgs::msg::PointCloud2 & cloud = *(  clearing_observation.cloud_ );
          
           // get the map coordinates of the origin of the sensor
           unsigned int x0,   y0;
           if (  !worldToMap(  ox,   oy,   x0,   y0 ) ) {
           RCLCPP_WARN(  
           logger_,  
           "Sensor origin at (  %.2f,   %.2f ) is out of map bounds (  %.2f,   %.2f ) to (  %.2f,   %.2f ). "
           "The costmap cannot raytrace for it.",  
           ox,   oy,  
           origin_x_,   origin_y_,  
           origin_x_ + getSizeInMetersX(   ),   origin_y_ + getSizeInMetersY(   ) );
           return;
           }
          
           // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
           double origin_x = origin_x_,   origin_y = origin_y_;
           double map_end_x = origin_x + size_x_ * resolution_;
           double map_end_y = origin_y + size_y_ * resolution_;
          
          
           touch(  ox,   oy,   min_x,   min_y,   max_x,   max_y );
          
           // for each point in the cloud,   we want to trace a line from the origin
           // and clear obstacles along it
           sensor_msgs::PointCloud2ConstIterator<float> iter_x(  cloud,   "x" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_y(  cloud,   "y" );
          
           for (  ; iter_x != iter_x.end(   ); ++iter_x,   ++iter_y ) {
           double wx = *iter_x;
           double wy = *iter_y;
          
           // now we also need to make sure that the enpoint we're raytracing
           // to isn't off the costmap and scale if necessary
           double a = wx - ox;
           double b = wy - oy;
          
           // the minimum value to raytrace from is the origin
           if (  wx < origin_x ) {
           double t = (  origin_x - ox ) / a;
           wx = origin_x;
           wy = oy + b * t;
           }
           if (  wy < origin_y ) {
           double t = (  origin_y - oy ) / b;
           wx = ox + a * t;
           wy = origin_y;
           }
          
           // the maximum value to raytrace to is the end of the map
           if (  wx > map_end_x ) {
           double t = (  map_end_x - ox ) / a;
           wx = map_end_x - .001;
           wy = oy + b * t;
           }
           if (  wy > map_end_y ) {
           double t = (  map_end_y - oy ) / b;
           wx = ox + a * t;
           wy = map_end_y - .001;
           }
          
           // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
           unsigned int x1,   y1;
          
           // check for legality just in case
           if (  !worldToMap(  wx,   wy,   x1,   y1 ) ) {
           continue;
           }
          
           unsigned int cell_raytrace_max_range = cellDistance(  clearing_observation.raytrace_max_range_ );
           unsigned int cell_raytrace_min_range = cellDistance(  clearing_observation.raytrace_min_range_ );
           MarkCell marker(  costmap_,   FREE_SPACE );
           // and finally... we can execute our trace to clear obstacles along that line
           raytraceLine(  marker,   x0,   y0,   x1,   y1,   cell_raytrace_max_range,   cell_raytrace_min_range );
          
           updateRaytraceBounds(  
           ox,   oy,   wx,   wy,   clearing_observation.raytrace_max_range_,  
           clearing_observation.raytrace_min_range_,   min_x,   min_y,   max_x,  
           max_y );
           }
          }
          
          void
     694  ObstacleLayer::activate(   )
          {
           for (  auto & notifier : observation_notifiers_ ) {
           notifier->clear(   );
           }
          
           // if we're stopped we need to re-subscribe to topics
           for (  unsigned int i = 0; i < observation_subscribers_.size(   ); ++i ) {
           if (  observation_subscribers_[i] != NULL ) {
           observation_subscribers_[i]->subscribe(   );
           }
           }
           resetBuffersLastUpdated(   );
          }
          
          void
     710  ObstacleLayer::deactivate(   )
          {
           for (  unsigned int i = 0; i < observation_subscribers_.size(   ); ++i ) {
           if (  observation_subscribers_[i] != NULL ) {
           observation_subscribers_[i]->unsubscribe(   );
           }
           }
          }
          
          void
     720  ObstacleLayer::updateRaytraceBounds(  
           double ox,   double oy,   double wx,   double wy,   double max_range,   double min_range,  
           double * min_x,   double * min_y,   double * max_x,   double * max_y )
          {
           double dx = wx - ox,   dy = wy - oy;
           double full_distance = hypot(  dx,   dy );
           if (  full_distance < min_range ) {
           return;
           }
           double scale = std::min(  1.0,   max_range / full_distance );
           double ex = ox + dx * scale,   ey = oy + dy * scale;
           touch(  ex,   ey,   min_x,   min_y,   max_x,   max_y );
          }
          
          void
     735  ObstacleLayer::reset(   )
          {
           resetMaps(   );
           resetBuffersLastUpdated(   );
           current_ = false;
           was_reset_ = true;
          }
          
          void
     744  ObstacleLayer::resetBuffersLastUpdated(   )
          {
           for (  unsigned int i = 0; i < observation_buffers_.size(   ); ++i ) {
           if (  observation_buffers_[i] ) {
           observation_buffers_[i]->resetLastUpdated(   );
           }
           }
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/range_sensor_layer.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018 David V. Lu!!
           * Copyright (  c ) 2020,   Bytes Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <angles/angles.h>
          #include <algorithm>
          #include <list>
          #include <limits>
          #include <string>
          #include <vector>
          
          #include "pluginlib/class_list_macros.hpp"
          #include "geometry_msgs/msg/point_stamped.hpp"
          #include "nav2_costmap_2d/range_sensor_layer.hpp"
          
      47  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::RangeSensorLayer,   nav2_costmap_2d::Layer )
          
          using nav2_costmap_2d::LETHAL_OBSTACLE;
          using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
          using nav2_costmap_2d::NO_INFORMATION;
          
          using namespace std::literals::chrono_literals;
          
          namespace nav2_costmap_2d
          {
          
      58  RangeSensorLayer::RangeSensorLayer(   ) {}
          
      60  void RangeSensorLayer::onInitialize(   )
          {
           current_ = true;
           was_reset_ = false;
           buffered_readings_ = 0;
           last_reading_time_ = clock_->now(   );
           default_value_ = to_cost(  0.5 );
          
           matchSize(   );
           resetRange(   );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           declareParameter(  "phi",   rclcpp::ParameterValue(  1.2 ) );
           node->get_parameter(  name_ + "." + "phi",   phi_v_ );
           declareParameter(  "inflate_cone",   rclcpp::ParameterValue(  1.0 ) );
           node->get_parameter(  name_ + "." + "inflate_cone",   inflate_cone_ );
           declareParameter(  "no_readings_timeout",   rclcpp::ParameterValue(  0.0 ) );
           node->get_parameter(  name_ + "." + "no_readings_timeout",   no_readings_timeout_ );
           declareParameter(  "clear_threshold",   rclcpp::ParameterValue(  0.2 ) );
           node->get_parameter(  name_ + "." + "clear_threshold",   clear_threshold_ );
           declareParameter(  "mark_threshold",   rclcpp::ParameterValue(  0.8 ) );
           node->get_parameter(  name_ + "." + "mark_threshold",   mark_threshold_ );
           declareParameter(  "clear_on_max_reading",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name_ + "." + "clear_on_max_reading",   clear_on_max_reading_ );
          
           double temp_tf_tol = 0.0;
           node->get_parameter(  "transform_tolerance",   temp_tf_tol );
           transform_tolerance_ = tf2::durationFromSec(  temp_tf_tol );
          
           std::vector<std::string> topic_names{};
           declareParameter(  "topics",   rclcpp::ParameterValue(  topic_names ) );
           node->get_parameter(  name_ + "." + "topics",   topic_names );
          
           InputSensorType input_sensor_type = InputSensorType::ALL;
           std::string sensor_type_name;
           declareParameter(  "input_sensor_type",   rclcpp::ParameterValue(  "ALL" ) );
           node->get_parameter(  name_ + "." + "input_sensor_type",   sensor_type_name );
          
           std::transform(  
           sensor_type_name.begin(   ),   sensor_type_name.end(   ),  
           sensor_type_name.begin(   ),   ::toupper );
           RCLCPP_INFO(  
           logger_,   "%s: %s as input_sensor_type given",  
           name_.c_str(   ),   sensor_type_name.c_str(   ) );
          
           if (  sensor_type_name == "VARIABLE" ) {
           input_sensor_type = InputSensorType::VARIABLE;
           } else if (  sensor_type_name == "FIXED" ) {
           input_sensor_type = InputSensorType::FIXED;
           } else if (  sensor_type_name == "ALL" ) {
           input_sensor_type = InputSensorType::ALL;
           } else {
           RCLCPP_ERROR(  
           logger_,   "%s: Invalid input sensor type: %s. Defaulting to ALL.",  
           name_.c_str(   ),   sensor_type_name.c_str(   ) );
           }
          
           // Validate topic names list: it must be a (  normally non-empty ) list of strings
           if (  topic_names.empty(   ) ) {
           RCLCPP_FATAL(  
           logger_,   "Invalid topic names list: it must"
           "be a non-empty list of strings" );
           return;
           }
          
           // Traverse the topic names list subscribing to all of them with the same callback method
           for (  auto & topic_name : topic_names ) {
           if (  input_sensor_type == InputSensorType::VARIABLE ) {
           processRangeMessageFunc_ = std::bind(  
           &RangeSensorLayer::processVariableRangeMsg,   this,  
           std::placeholders::_1 );
           } else if (  input_sensor_type == InputSensorType::FIXED ) {
           processRangeMessageFunc_ = std::bind(  
           &RangeSensorLayer::processFixedRangeMsg,   this,  
           std::placeholders::_1 );
           } else if (  input_sensor_type == InputSensorType::ALL ) {
           processRangeMessageFunc_ = std::bind(  
           &RangeSensorLayer::processRangeMsg,   this,  
           std::placeholders::_1 );
           } else {
           RCLCPP_ERROR(  
           logger_,  
           "%s: Invalid input sensor type: %s. Did you make a new type"
           "and forgot to choose the subscriber for it?",  
           name_.c_str(   ),   sensor_type_name.c_str(   ) );
           }
           range_subs_.push_back(  
           node->create_subscription<sensor_msgs::msg::Range>(  
           topic_name,   rclcpp::SensorDataQoS(   ),   std::bind(  
           &RangeSensorLayer::bufferIncomingRangeMsg,   this,  
           std::placeholders::_1 ) ) );
          
           RCLCPP_INFO(  
           logger_,   "RangeSensorLayer: subscribed to "
           "topic %s",   range_subs_.back(   )->get_topic_name(   ) );
           }
           global_frame_ = layered_costmap_->getGlobalFrameID(   );
          }
          
          
     166  double RangeSensorLayer::gamma(  double theta )
          {
           if (  fabs(  theta ) > max_angle_ ) {
           return 0.0;
           } else {
           return 1 - pow(  theta / max_angle_,   2 );
           }
          }
          
     175  double RangeSensorLayer::delta(  double phi )
          {
           return 1 - (  1 + tanh(  2 * (  phi - phi_v_ ) ) ) / 2;
          }
          
     180  void RangeSensorLayer::get_deltas(  double angle,   double * dx,   double * dy )
          {
           double ta = tan(  angle );
           if (  ta == 0 ) {
           *dx = 0;
           } else {
           *dx = resolution_ / ta;
           }
          
           *dx = copysign(  *dx,   cos(  angle ) );
           *dy = copysign(  resolution_,   sin(  angle ) );
          }
          
     193  double RangeSensorLayer::sensor_model(  double r,   double phi,   double theta )
          {
           double lbda = delta(  phi ) * gamma(  theta );
          
           double delta = resolution_;
          
           if (  phi >= 0.0 && phi < r - 2 * delta * r ) {
           return (  1 - lbda ) * (  0.5 );
           } else if (  phi < r - delta * r ) {
           return lbda * 0.5 * pow(  (  phi - (  r - 2 * delta * r ) ) / (  delta * r ),   2 ) +
           (  1 - lbda ) * .5;
           } else if (  phi < r + delta * r ) {
           double J = (  r - phi ) / (  delta * r );
           return lbda * (  (  1 - (  0.5 ) * pow(  J,   2 ) ) - 0.5 ) + 0.5;
           } else {
           return 0.5;
           }
          }
          
     212  void RangeSensorLayer::bufferIncomingRangeMsg(  
     213   const sensor_msgs::msg::Range::SharedPtr range_message )
          {
           range_message_mutex_.lock(   );
           range_msgs_buffer_.push_back(  *range_message );
           range_message_mutex_.unlock(   );
          }
          
     220  void RangeSensorLayer::updateCostmap(   )
          {
           std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
          
           range_message_mutex_.lock(   );
           range_msgs_buffer_copy = std::list<sensor_msgs::msg::Range>(  range_msgs_buffer_ );
           range_msgs_buffer_.clear(   );
           range_message_mutex_.unlock(   );
          
           for (  auto & range_msgs_it : range_msgs_buffer_copy ) {
           processRangeMessageFunc_(  range_msgs_it );
           }
          }
          
     234  void RangeSensorLayer::processRangeMsg(  sensor_msgs::msg::Range & range_message )
          {
           if (  range_message.min_range == range_message.max_range ) {
           processFixedRangeMsg(  range_message );
           } else {
           processVariableRangeMsg(  range_message );
           }
          }
          
     243  void RangeSensorLayer::processFixedRangeMsg(  sensor_msgs::msg::Range & range_message )
          {
           if (  !std::isinf(  range_message.range ) ) {
           RCLCPP_ERROR(  
           logger_,  
           "Fixed distance ranger (  min_range == max_range ) in frame %s sent invalid value. "
           "Only -Inf (  == object detected ) and Inf (  == no object detected ) are valid.",  
           range_message.header.frame_id.c_str(   ) );
           return;
           }
          
           bool clear_sensor_cone = false;
          
           if (  range_message.range > 0 ) { // +inf
           if (  !clear_on_max_reading_ ) {
           return; // no clearing at all
           }
           clear_sensor_cone = true;
           }
          
           range_message.range = range_message.min_range;
          
           updateCostmap(  range_message,   clear_sensor_cone );
          }
          
     268  void RangeSensorLayer::processVariableRangeMsg(  sensor_msgs::msg::Range & range_message )
          {
           if (  range_message.range < range_message.min_range || range_message.range >
           range_message.max_range )
           {
           return;
           }
          
           bool clear_sensor_cone = false;
          
           if (  range_message.range >= range_message.max_range && clear_on_max_reading_ ) {
           clear_sensor_cone = true;
           }
          
           updateCostmap(  range_message,   clear_sensor_cone );
          }
          
     285  void RangeSensorLayer::updateCostmap(  
     286   sensor_msgs::msg::Range & range_message,  
     287   bool clear_sensor_cone )
          {
           max_angle_ = range_message.field_of_view / 2;
          
           geometry_msgs::msg::PointStamped in,   out;
           in.header.stamp = range_message.header.stamp;
           in.header.frame_id = range_message.header.frame_id;
          
           if (  !tf_->canTransform(  
           in.header.frame_id,   global_frame_,  
           tf2_ros::fromMsg(  in.header.stamp ),  
           tf2_ros::fromRclcpp(  transform_tolerance_ ) ) )
           {
           RCLCPP_INFO(  
           logger_,   "Range sensor layer can't transform from %s to %s",  
           global_frame_.c_str(   ),   in.header.frame_id.c_str(   ) );
           return;
           }
          
           tf_->transform(  in,   out,   global_frame_,   transform_tolerance_ );
          
           double ox = out.point.x,   oy = out.point.y;
          
           in.point.x = range_message.range;
          
           tf_->transform(  in,   out,   global_frame_,   transform_tolerance_ );
          
           double tx = out.point.x,   ty = out.point.y;
          
           // calculate target props
           double dx = tx - ox,   dy = ty - oy,   theta = atan2(  dy,   dx ),   d = sqrt(  dx * dx + dy * dy );
          
           // Integer Bounds of Update
           int bx0,   by0,   bx1,   by1;
          
           // Triangle that will be really updated; the other cells within bounds are ignored
           // This triangle is formed by the origin and left and right sides of sonar cone
           int Ox,   Oy,   Ax,   Ay,   Bx,   By;
          
           // Bounds includes the origin
           worldToMapNoBounds(  ox,   oy,   Ox,   Oy );
           bx1 = bx0 = Ox;
           by1 = by0 = Oy;
           touch(  ox,   oy,   &min_x_,   &min_y_,   &max_x_,   &max_y_ );
          
           // Update Map with Target Point
           unsigned int aa,   ab;
           if (  worldToMap(  tx,   ty,   aa,   ab ) ) {
           setCost(  aa,   ab,   233 );
           touch(  tx,   ty,   &min_x_,   &min_y_,   &max_x_,   &max_y_ );
           }
          
           double mx,   my;
          
           // Update left side of sonar cone
           mx = ox + cos(  theta - max_angle_ ) * d * 1.2;
           my = oy + sin(  theta - max_angle_ ) * d * 1.2;
           worldToMapNoBounds(  mx,   my,   Ax,   Ay );
           bx0 = std::min(  bx0,   Ax );
           bx1 = std::max(  bx1,   Ax );
           by0 = std::min(  by0,   Ay );
           by1 = std::max(  by1,   Ay );
           touch(  mx,   my,   &min_x_,   &min_y_,   &max_x_,   &max_y_ );
          
           // Update right side of sonar cone
           mx = ox + cos(  theta + max_angle_ ) * d * 1.2;
           my = oy + sin(  theta + max_angle_ ) * d * 1.2;
          
           worldToMapNoBounds(  mx,   my,   Bx,   By );
           bx0 = std::min(  bx0,   Bx );
           bx1 = std::max(  bx1,   Bx );
           by0 = std::min(  by0,   By );
           by1 = std::max(  by1,   By );
           touch(  mx,   my,   &min_x_,   &min_y_,   &max_x_,   &max_y_ );
          
           // Limit Bounds to Grid
           bx0 = std::max(  0,   bx0 );
           by0 = std::max(  0,   by0 );
           bx1 = std::min(  static_cast<int>(  size_x_ ),   bx1 );
           by1 = std::min(  static_cast<int>(  size_y_ ),   by1 );
          
           for (  unsigned int x = bx0; x <= (  unsigned int )bx1; x++ ) {
           for (  unsigned int y = by0; y <= (  unsigned int )by1; y++ ) {
           bool update_xy_cell = true;
          
           // Unless inflate_cone_ is set to 100 %,   we update cells only within the
           // (  partially inflated ) sensor cone,   projected on the costmap as a triangle.
           // 0 % corresponds to just the triangle,   but if your sensor fov is very
           // narrow,   the covered area can become zero due to cell discretization.
           // See wiki description for more details
           if (  inflate_cone_ < 1.0 ) {
           // Determine barycentric coordinates
           int w0 = orient2d(  Ax,   Ay,   Bx,   By,   x,   y );
           int w1 = orient2d(  Bx,   By,   Ox,   Oy,   x,   y );
           int w2 = orient2d(  Ox,   Oy,   Ax,   Ay,   x,   y );
          
           // Barycentric coordinates inside area threshold; this is not mathematically
           // sound at all,   but it works!
           float bcciath = -static_cast<float>(  inflate_cone_ ) * area(  Ax,   Ay,   Bx,   By,   Ox,   Oy );
           update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
           }
          
           if (  update_xy_cell ) {
           double wx,   wy;
           mapToWorld(  x,   y,   wx,   wy );
           update_cell(  ox,   oy,   theta,   range_message.range,   wx,   wy,   clear_sensor_cone );
           }
           }
           }
          
           buffered_readings_++;
           last_reading_time_ = clock_->now(   );
          }
          
     401  void RangeSensorLayer::update_cell(  
           double ox,   double oy,   double ot,   double r,  
     403   double nx,   double ny,   bool clear )
          {
           unsigned int x,   y;
           if (  worldToMap(  nx,   ny,   x,   y ) ) {
           double dx = nx - ox,   dy = ny - oy;
           double theta = atan2(  dy,   dx ) - ot;
           theta = angles::normalize_angle(  theta );
           double phi = sqrt(  dx * dx + dy * dy );
           double sensor = 0.0;
           if (  !clear ) {
           sensor = sensor_model(  r,   phi,   theta );
           }
           double prior = to_prob(  getCost(  x,   y ) );
           double prob_occ = sensor * prior;
           double prob_not = (  1 - sensor ) * (  1 - prior );
           double new_prob = prob_occ / (  prob_occ + prob_not );
          
           RCLCPP_DEBUG(  
           logger_,  
           "%f %f | %f %f = %f",   dx,   dy,   theta,   phi,   sensor );
           RCLCPP_DEBUG(  
           logger_,  
           "%f | %f %f | %f",   prior,   prob_occ,   prob_not,   new_prob );
           unsigned char c = to_cost(  new_prob );
           setCost(  x,   y,   c );
           }
          }
          
     431  void RangeSensorLayer::resetRange(   )
          {
           min_x_ = min_y_ = std::numeric_limits<double>::max(   );
           max_x_ = max_y_ = -std::numeric_limits<double>::max(   );
          }
          
     437  void RangeSensorLayer::updateBounds(  
           double robot_x,   double robot_y,  
           double robot_yaw,   double * min_x,   double * min_y,  
           double * max_x,   double * max_y )
          {
           robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use
           if (  layered_costmap_->isRolling(   ) ) {
           updateOrigin(  robot_x - getSizeInMetersX(   ) / 2,   robot_y - getSizeInMetersY(   ) / 2 );
           }
          
           updateCostmap(   );
          
           *min_x = std::min(  *min_x,   min_x_ );
           *min_y = std::min(  *min_y,   min_y_ );
           *max_x = std::max(  *max_x,   max_x_ );
           *max_y = std::max(  *max_y,   max_y_ );
          
           resetRange(   );
          
           if (  !enabled_ ) {
           current_ = true;
           return;
           }
          
           if (  buffered_readings_ == 0 ) {
           if (  no_readings_timeout_ > 0.0 &&
           (  clock_->now(   ) - last_reading_time_ ).seconds(   ) >
           no_readings_timeout_ )
           {
           RCLCPP_WARN(  
           logger_,  
           "No range readings received for %.2f seconds,   while expected at least every %.2f seconds.",  
           (  clock_->now(   ) - last_reading_time_ ).seconds(   ),  
           no_readings_timeout_ );
           current_ = false;
           }
           }
          }
          
     476  void RangeSensorLayer::updateCosts(  
     477   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
          
           unsigned char * master_array = master_grid.getCharMap(   );
           unsigned int span = master_grid.getSizeInCellsX(   );
           unsigned char clear = to_cost(  clear_threshold_ ),   mark = to_cost(  mark_threshold_ );
          
           for (  int j = min_j; j < max_j; j++ ) {
           unsigned int it = j * span + min_i;
           for (  int i = min_i; i < max_i; i++ ) {
           unsigned char prob = costmap_[it];
           unsigned char current;
           if (  prob == nav2_costmap_2d::NO_INFORMATION ) {
           it++;
           continue;
           } else if (  prob > mark ) {
           current = nav2_costmap_2d::LETHAL_OBSTACLE;
           } else if (  prob < clear ) {
           current = nav2_costmap_2d::FREE_SPACE;
           } else {
           it++;
           continue;
           }
          
           unsigned char old_cost = master_array[it];
          
           if (  old_cost == NO_INFORMATION || old_cost < current ) {
           master_array[it] = current;
           }
           it++;
           }
           }
          
           buffered_readings_ = 0;
          
           // if not current due to reset,   set current now after clearing
           if (  !current_ && was_reset_ ) {
           was_reset_ = false;
           current_ = true;
           }
          }
          
     523  void RangeSensorLayer::reset(   )
          {
           RCLCPP_DEBUG(  logger_,   "Reseting range sensor layer..." );
           deactivate(   );
           resetMaps(   );
           was_reset_ = true;
           activate(   );
          }
          
     532  void RangeSensorLayer::deactivate(   )
          {
           range_msgs_buffer_.clear(   );
          }
          
     537  void RangeSensorLayer::activate(   )
          {
           range_msgs_buffer_.clear(   );
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/static_layer.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * Copyright (  c ) 2015,   Fetch Robotics,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          
          #include "nav2_costmap_2d/static_layer.hpp"
          
          #include <algorithm>
          #include <string>
          
          #include "pluginlib/class_list_macros.hpp"
          #include "tf2/convert.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          
      49  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::StaticLayer,   nav2_costmap_2d::Layer )
          
          using nav2_costmap_2d::NO_INFORMATION;
          using nav2_costmap_2d::LETHAL_OBSTACLE;
          using nav2_costmap_2d::FREE_SPACE;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_costmap_2d
          {
          
      59  StaticLayer::StaticLayer(   )
          : map_buffer_(  nullptr )
          {
          }
          
      64  StaticLayer::~StaticLayer(   )
          {
          }
          
          void
      69  StaticLayer::onInitialize(   )
          {
           global_frame_ = layered_costmap_->getGlobalFrameID(   );
          
           getParameters(   );
          
           rclcpp::QoS map_qos(  10 ); // initialize to default
           if (  map_subscribe_transient_local_ ) {
           map_qos.transient_local(   );
           map_qos.reliable(   );
           map_qos.keep_last(  1 );
           }
          
           RCLCPP_INFO(  
           logger_,  
           "Subscribing to the map topic (  %s ) with %s durability",  
           map_topic_.c_str(   ),  
           map_subscribe_transient_local_ ? "transient local" : "volatile" );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(  
           map_topic_,   map_qos,  
           std::bind(  &StaticLayer::incomingMap,   this,   std::placeholders::_1 ) );
          
           if (  subscribe_to_updates_ ) {
           RCLCPP_INFO(  logger_,   "Subscribing to updates" );
           map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(  
           map_topic_ + "_updates",  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &StaticLayer::incomingUpdate,   this,   std::placeholders::_1 ) );
           }
          }
          
          void
     107  StaticLayer::activate(   )
          {
          }
          
          void
     112  StaticLayer::deactivate(   )
          {
           dyn_params_handler_.reset(   );
          }
          
          void
     118  StaticLayer::reset(   )
          {
           has_updated_data_ = true;
           current_ = false;
          }
          
          void
     125  StaticLayer::getParameters(   )
          {
           int temp_lethal_threshold = 0;
           double temp_tf_tol = 0.0;
          
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "subscribe_to_updates",   rclcpp::ParameterValue(  false ) );
           declareParameter(  "map_subscribe_transient_local",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  "map_topic",   rclcpp::ParameterValue(  "" ) );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           node->get_parameter(  name_ + "." + "subscribe_to_updates",   subscribe_to_updates_ );
           std::string private_map_topic,   global_map_topic;
           node->get_parameter(  name_ + "." + "map_topic",   private_map_topic );
           node->get_parameter(  "map_topic",   global_map_topic );
           if (  !private_map_topic.empty(   ) ) {
           map_topic_ = private_map_topic;
           } else {
           map_topic_ = global_map_topic;
           }
           node->get_parameter(  
           name_ + "." + "map_subscribe_transient_local",  
           map_subscribe_transient_local_ );
           node->get_parameter(  "track_unknown_space",   track_unknown_space_ );
           node->get_parameter(  "use_maximum",   use_maximum_ );
           node->get_parameter(  "lethal_cost_threshold",   temp_lethal_threshold );
           node->get_parameter(  "unknown_cost_value",   unknown_cost_value_ );
           node->get_parameter(  "trinary_costmap",   trinary_costmap_ );
           node->get_parameter(  "transform_tolerance",   temp_tf_tol );
          
           // Enforce bounds
           lethal_threshold_ = std::max(  std::min(  temp_lethal_threshold,   100 ),   0 );
           map_received_ = false;
           update_in_progress_.store(  false );
          
           transform_tolerance_ = tf2::durationFromSec(  temp_tf_tol );
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &StaticLayer::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
          }
          
          void
     176  StaticLayer::processMap(  const nav_msgs::msg::OccupancyGrid & new_map )
          {
           RCLCPP_DEBUG(  logger_,   "StaticLayer: Process map" );
          
           unsigned int size_x = new_map.info.width;
           unsigned int size_y = new_map.info.height;
          
           RCLCPP_DEBUG(  
           logger_,  
           "StaticLayer: Received a %d X %d map at %f m/pix",   size_x,   size_y,  
           new_map.info.resolution );
          
           // resize costmap if size,   resolution or origin do not match
           Costmap2D * master = layered_costmap_->getCostmap(   );
           if (  !layered_costmap_->isRolling(   ) && (  master->getSizeInCellsX(   ) != size_x ||
           master->getSizeInCellsY(   ) != size_y ||
           master->getResolution(   ) != new_map.info.resolution ||
           master->getOriginX(   ) != new_map.info.origin.position.x ||
           master->getOriginY(   ) != new_map.info.origin.position.y ||
           !layered_costmap_->isSizeLocked(   ) ) )
           {
           // Update the size of the layered costmap (  and all layers,   including this one )
           RCLCPP_INFO(  
           logger_,  
           "StaticLayer: Resizing costmap to %d X %d at %f m/pix",   size_x,   size_y,  
           new_map.info.resolution );
           layered_costmap_->resizeMap(  
           size_x,   size_y,   new_map.info.resolution,  
           new_map.info.origin.position.x,  
           new_map.info.origin.position.y,  
           true );
           } else if (  size_x_ != size_x || size_y_ != size_y || // NOLINT
           resolution_ != new_map.info.resolution ||
           origin_x_ != new_map.info.origin.position.x ||
           origin_y_ != new_map.info.origin.position.y )
           {
           // only update the size of the costmap stored locally in this layer
           RCLCPP_INFO(  
           logger_,  
           "StaticLayer: Resizing static layer to %d X %d at %f m/pix",   size_x,   size_y,  
           new_map.info.resolution );
           resizeMap(  
           size_x,   size_y,   new_map.info.resolution,  
           new_map.info.origin.position.x,   new_map.info.origin.position.y );
           }
          
           unsigned int index = 0;
          
           // we have a new map,   update full size of map
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
          
           // initialize the costmap with static data
           for (  unsigned int i = 0; i < size_y; ++i ) {
           for (  unsigned int j = 0; j < size_x; ++j ) {
           unsigned char value = new_map.data[index];
           costmap_[index] = interpretValue(  value );
           ++index;
           }
           }
          
           map_frame_ = new_map.header.frame_id;
          
           x_ = y_ = 0;
           width_ = size_x_;
           height_ = size_y_;
           has_updated_data_ = true;
          
           current_ = true;
          }
          
          void
     247  StaticLayer::matchSize(   )
          {
           // If we are using rolling costmap,   the static map size is
           // unrelated to the size of the layered costmap
           if (  !layered_costmap_->isRolling(   ) ) {
           Costmap2D * master = layered_costmap_->getCostmap(   );
           resizeMap(  
           master->getSizeInCellsX(   ),   master->getSizeInCellsY(   ),   master->getResolution(   ),  
           master->getOriginX(   ),   master->getOriginY(   ) );
           }
          }
          
          unsigned char
     260  StaticLayer::interpretValue(  unsigned char value )
          {
           // check if the static value is above the unknown or lethal thresholds
           if (  track_unknown_space_ && value == unknown_cost_value_ ) {
           return NO_INFORMATION;
           } else if (  !track_unknown_space_ && value == unknown_cost_value_ ) {
           return FREE_SPACE;
           } else if (  value >= lethal_threshold_ ) {
           return LETHAL_OBSTACLE;
           } else if (  trinary_costmap_ ) {
           return FREE_SPACE;
           }
          
           double scale = static_cast<double>(  value ) / lethal_threshold_;
           return scale * LETHAL_OBSTACLE;
          }
          
          void
     278  StaticLayer::incomingMap(  const nav_msgs::msg::OccupancyGrid::SharedPtr new_map )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  !map_received_ ) {
           map_received_ = true;
           processMap(  *new_map );
           }
           if (  update_in_progress_.load(   ) ) {
           map_buffer_ = new_map;
           } else {
           processMap(  *new_map );
           map_buffer_ = nullptr;
           }
          }
          
          void
     294  StaticLayer::incomingUpdate(  map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  update->y < static_cast<int32_t>(  y_ ) ||
           y_ + height_ < update->y + update->height ||
           update->x < static_cast<int32_t>(  x_ ) ||
           x_ + width_ < update->x + update->width )
           {
           RCLCPP_WARN(  
           logger_,  
           "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
           "Static layer origin: %d,   %d bounds: %d X %d\n"
           "Update origin: %d,   %d bounds: %d X %d",  
           x_,   y_,   width_,   height_,   update->x,   update->y,   update->width,  
           update->height );
           return;
           }
          
           if (  update->header.frame_id != map_frame_ ) {
           RCLCPP_WARN(  
           logger_,  
           "StaticLayer: Map update ignored. Current map is in frame %s "
           "but update was in frame %s",  
           map_frame_.c_str(   ),   update->header.frame_id.c_str(   ) );
           }
          
           unsigned int di = 0;
           for (  unsigned int y = 0; y < update->height; y++ ) {
           unsigned int index_base = (  update->y + y ) * size_x_;
           for (  unsigned int x = 0; x < update->width; x++ ) {
           unsigned int index = index_base + x + update->x;
           costmap_[index] = interpretValue(  update->data[di++] );
           }
           }
          
           has_updated_data_ = true;
          }
          
          
          void
     334  StaticLayer::updateBounds(  
           double /*robot_x*/,   double /*robot_y*/,   double /*robot_yaw*/,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y )
          {
           if (  !map_received_ ) {
           return;
           }
          
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           update_in_progress_.store(  true );
          
           // If there is a new available map,   load it.
           if (  map_buffer_ ) {
           processMap(  *map_buffer_ );
           map_buffer_ = nullptr;
           }
          
           if (  !layered_costmap_->isRolling(   )  ) {
           if (  !(  has_updated_data_ || has_extra_bounds_ ) ) {
           return;
           }
           }
          
           useExtraBounds(  min_x,   min_y,   max_x,   max_y );
          
           double wx,   wy;
          
           mapToWorld(  x_,   y_,   wx,   wy );
           *min_x = std::min(  wx,   *min_x );
           *min_y = std::min(  wy,   *min_y );
          
           mapToWorld(  x_ + width_,   y_ + height_,   wx,   wy );
           *max_x = std::max(  wx,   *max_x );
           *max_y = std::max(  wy,   *max_y );
          
           has_updated_data_ = false;
          }
          
          void
     375  StaticLayer::updateCosts(  
     376   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           if (  !enabled_ ) {
           update_in_progress_.store(  false );
           return;
           }
           if (  !map_received_ ) {
           static int count = 0;
           // throttle warning down to only 1/10 message rate
           if (  ++count == 10 ) {
           RCLCPP_WARN(  logger_,   "Can't update static costmap layer,   no map received" );
           count = 0;
           }
           update_in_progress_.store(  false );
           return;
           }
          
           if (  !layered_costmap_->isRolling(   ) ) {
           // if not rolling,   the layered costmap (  master_grid ) has same coordinates as this layer
           if (  !use_maximum_ ) {
           updateWithTrueOverwrite(  master_grid,   min_i,   min_j,   max_i,   max_j );
           } else {
           updateWithMax(  master_grid,   min_i,   min_j,   max_i,   max_j );
           }
           } else {
           // If rolling window,   the master_grid is unlikely to have same coordinates as this layer
           unsigned int mx,   my;
           double wx,   wy;
           // Might even be in a different frame
           geometry_msgs::msg::TransformStamped transform;
           try {
           transform = tf_->lookupTransform(  
           map_frame_,   global_frame_,   tf2::TimePointZero,  
           transform_tolerance_ );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  logger_,   "StaticLayer: %s",   ex.what(   ) );
           update_in_progress_.store(  false );
           return;
           }
           // Copy map data given proper transformations
           tf2::Transform tf2_transform;
           tf2::fromMsg(  transform.transform,   tf2_transform );
          
           for (  int i = min_i; i < max_i; ++i ) {
           for (  int j = min_j; j < max_j; ++j ) {
           // Convert master_grid coordinates (  i,  j ) into global_frame_(  wx,  wy ) coordinates
           layered_costmap_->getCostmap(   )->mapToWorld(  i,   j,   wx,   wy );
           // Transform from global_frame_ to map_frame_
           tf2::Vector3 p(  wx,   wy,   0 );
           p = tf2_transform * p;
           // Set master_grid with cell from map
           if (  worldToMap(  p.x(   ),   p.y(   ),   mx,   my ) ) {
           if (  !use_maximum_ ) {
           master_grid.setCost(  i,   j,   getCost(  mx,   my ) );
           } else {
           master_grid.setCost(  i,   j,   std::max(  getCost(  mx,   my ),   master_grid.getCost(  i,   j ) ) );
           }
           }
           }
           }
           }
           update_in_progress_.store(  false );
           current_ = true;
          }
          
          /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
          rcl_interfaces::msg::SetParametersResult
     448  StaticLayer::dynamicParametersCallback(  
     449   std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           rcl_interfaces::msg::SetParametersResult result;
          
           for (  auto parameter : parameters ) {
           const auto & param_type = parameter.get_type(   );
           const auto & param_name = parameter.get_name(   );
          
           if (  param_name == name_ + "." + "map_subscribe_transient_local" ||
           param_name == name_ + "." + "map_topic" ||
           param_name == name_ + "." + "subscribe_to_updates" )
           {
           RCLCPP_WARN(  
           logger_,   "%s is not a dynamic parameter "
           "cannot be changed while running. Rejecting parameter update.",   param_name.c_str(   ) );
           } else if (  param_type == ParameterType::PARAMETER_DOUBLE ) {
           if (  param_name == name_ + "." + "transform_tolerance" ) {
           transform_tolerance_ = tf2::durationFromSec(  parameter.as_double(   ) );
           }
           } else if (  param_type == ParameterType::PARAMETER_BOOL ) {
           if (  param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool(   ) ) {
           enabled_ = parameter.as_bool(   );
          
           x_ = y_ = 0;
           width_ = size_x_;
           height_ = size_y_;
           has_updated_data_ = true;
           current_ = false;
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          
          #include "nav2_costmap_2d/voxel_layer.hpp"
          
          #include <algorithm>
          #include <cassert>
          #include <vector>
          #include <memory>
          #include <utility>
          
          #include "pluginlib/class_list_macros.hpp"
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          
          #define VOXEL_BITS 16
      51  PLUGINLIB_EXPORT_CLASS(  nav2_costmap_2d::VoxelLayer,   nav2_costmap_2d::Layer )
          
          using nav2_costmap_2d::NO_INFORMATION;
          using nav2_costmap_2d::LETHAL_OBSTACLE;
          using nav2_costmap_2d::FREE_SPACE;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_costmap_2d
          {
          
      61  void VoxelLayer::onInitialize(   )
          {
           ObstacleLayer::onInitialize(   );
          
           declareParameter(  "enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "footprint_clearing_enabled",   rclcpp::ParameterValue(  true ) );
           declareParameter(  "max_obstacle_height",   rclcpp::ParameterValue(  2.0 ) );
           declareParameter(  "z_voxels",   rclcpp::ParameterValue(  10 ) );
           declareParameter(  "origin_z",   rclcpp::ParameterValue(  0.0 ) );
           declareParameter(  "z_resolution",   rclcpp::ParameterValue(  0.2 ) );
           declareParameter(  "unknown_threshold",   rclcpp::ParameterValue(  15 ) );
           declareParameter(  "mark_threshold",   rclcpp::ParameterValue(  0 ) );
           declareParameter(  "combination_method",   rclcpp::ParameterValue(  1 ) );
           declareParameter(  "publish_voxel_map",   rclcpp::ParameterValue(  false ) );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           node->get_parameter(  name_ + "." + "enabled",   enabled_ );
           node->get_parameter(  name_ + "." + "footprint_clearing_enabled",   footprint_clearing_enabled_ );
           node->get_parameter(  name_ + "." + "max_obstacle_height",   max_obstacle_height_ );
           node->get_parameter(  name_ + "." + "z_voxels",   size_z_ );
           node->get_parameter(  name_ + "." + "origin_z",   origin_z_ );
           node->get_parameter(  name_ + "." + "z_resolution",   z_resolution_ );
           node->get_parameter(  name_ + "." + "unknown_threshold",   unknown_threshold_ );
           node->get_parameter(  name_ + "." + "mark_threshold",   mark_threshold_ );
           node->get_parameter(  name_ + "." + "combination_method",   combination_method_ );
           node->get_parameter(  name_ + "." + "publish_voxel_map",   publish_voxel_ );
          
           auto custom_qos = rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   );
          
           if (  publish_voxel_ ) {
           voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(  
           "voxel_grid",   custom_qos );
           voxel_pub_->on_activate(   );
           }
          
           clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(  
           "clearing_endpoints",   custom_qos );
           clearing_endpoints_pub_->on_activate(   );
          
           unknown_threshold_ += (  VOXEL_BITS - size_z_ );
           matchSize(   );
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &VoxelLayer::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
          }
          
     114  VoxelLayer::~VoxelLayer(   )
          {
           dyn_params_handler_.reset(   );
          }
          
     119  void VoxelLayer::matchSize(   )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           ObstacleLayer::matchSize(   );
           voxel_grid_.resize(  size_x_,   size_y_,   size_z_ );
           assert(  voxel_grid_.sizeX(   ) == size_x_ && voxel_grid_.sizeY(   ) == size_y_ );
          }
          
     127  void VoxelLayer::reset(   )
          {
           // Call the base class method before adding our own functionality
           ObstacleLayer::reset(   );
           resetMaps(   );
          }
          
     134  void VoxelLayer::resetMaps(   )
          {
           // Call the base class method before adding our own functionality
           // Note: at the time this was written,   ObstacleLayer doesn't implement
           // resetMaps so this goes to the next layer down Costmap2DLayer which also
           // doesn't implement this,   so it actually goes all the way to Costmap2D
           ObstacleLayer::resetMaps(   );
           voxel_grid_.reset(   );
          }
          
     144  void VoxelLayer::updateBounds(  
           double robot_x,   double robot_y,   double robot_yaw,   double * min_x,  
           double * min_y,   double * max_x,   double * max_y )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
          
           if (  rolling_window_ ) {
           updateOrigin(  robot_x - getSizeInMetersX(   ) / 2,   robot_y - getSizeInMetersY(   ) / 2 );
           }
           if (  !enabled_ ) {
           return;
           }
           useExtraBounds(  min_x,   min_y,   max_x,   max_y );
          
           bool current = true;
           std::vector<Observation> observations,   clearing_observations;
          
           // get the marking observations
           current = getMarkingObservations(  observations ) && current;
          
           // get the clearing observations
           current = getClearingObservations(  clearing_observations ) && current;
          
           // update the global current status
           current_ = current;
          
           // raytrace freespace
           for (  unsigned int i = 0; i < clearing_observations.size(   ); ++i ) {
           raytraceFreespace(  clearing_observations[i],   min_x,   min_y,   max_x,   max_y );
           }
          
           // place the new obstacles into a priority queue... each with a priority of zero to begin with
           for (  std::vector<Observation>::const_iterator it = observations.begin(   ); it != observations.end(   );
           ++it )
           {
           const Observation & obs = *it;
          
           const sensor_msgs::msg::PointCloud2 & cloud = *(  obs.cloud_ );
          
           double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
           double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
          
           sensor_msgs::PointCloud2ConstIterator<float> iter_x(  cloud,   "x" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_y(  cloud,   "y" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_z(  cloud,   "z" );
          
           for (  ; iter_x != iter_x.end(   ); ++iter_x,   ++iter_y,   ++iter_z ) {
           // if the obstacle is too high or too far away from the robot we won't add it
           if (  *iter_z > max_obstacle_height_ ) {
           continue;
           }
          
           // compute the squared distance from the hitpoint to the pointcloud's origin
           double sq_dist = (  *iter_x - obs.origin_.x ) * (  *iter_x - obs.origin_.x ) +
           (  *iter_y - obs.origin_.y ) * (  *iter_y - obs.origin_.y ) +
           (  *iter_z - obs.origin_.z ) * (  *iter_z - obs.origin_.z );
          
           // if the point is far enough away... we won't consider it
           if (  sq_dist >= sq_obstacle_max_range ) {
           continue;
           }
          
           // If the point is too close,   do not consider it
           if (  sq_dist < sq_obstacle_min_range ) {
           continue;
           }
          
           // now we need to compute the map coordinates for the observation
           unsigned int mx,   my,   mz;
           if (  *iter_z < origin_z_ ) {
           if (  !worldToMap3D(  *iter_x,   *iter_y,   origin_z_,   mx,   my,   mz ) ) {
           continue;
           }
           } else if (  !worldToMap3D(  *iter_x,   *iter_y,   *iter_z,   mx,   my,   mz ) ) {
           continue;
           }
          
           // mark the cell in the voxel grid and check if we should also mark it in the costmap
           if (  voxel_grid_.markVoxelInMap(  mx,   my,   mz,   mark_threshold_ ) ) {
           unsigned int index = getIndex(  mx,   my );
          
           costmap_[index] = LETHAL_OBSTACLE;
           touch(  
           static_cast<double>(  *iter_x ),   static_cast<double>(  *iter_y ),  
           min_x,   min_y,   max_x,   max_y );
           }
           }
           }
          
           if (  publish_voxel_ ) {
           auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>(   );
           unsigned int size = voxel_grid_.sizeX(   ) * voxel_grid_.sizeY(   );
           grid_msg->size_x = voxel_grid_.sizeX(   );
           grid_msg->size_y = voxel_grid_.sizeY(   );
           grid_msg->size_z = voxel_grid_.sizeZ(   );
           grid_msg->data.resize(  size );
           memcpy(  &grid_msg->data[0],   voxel_grid_.getData(   ),   size * sizeof(  unsigned int ) );
          
           grid_msg->origin.x = origin_x_;
           grid_msg->origin.y = origin_y_;
           grid_msg->origin.z = origin_z_;
          
           grid_msg->resolutions.x = resolution_;
           grid_msg->resolutions.y = resolution_;
           grid_msg->resolutions.z = z_resolution_;
           grid_msg->header.frame_id = global_frame_;
           grid_msg->header.stamp = clock_->now(   );
          
           voxel_pub_->publish(  std::move(  grid_msg ) );
           }
          
           updateFootprint(  robot_x,   robot_y,   robot_yaw,   min_x,   min_y,   max_x,   max_y );
          }
          
     258  void VoxelLayer::raytraceFreespace(  
     259   const Observation & clearing_observation,   double * min_x,  
           double * min_y,  
           double * max_x,  
           double * max_y )
          {
           auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>(   );
          
           if (  clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0 ) {
           return;
           }
          
           double sensor_x,   sensor_y,   sensor_z;
           double ox = clearing_observation.origin_.x;
           double oy = clearing_observation.origin_.y;
           double oz = clearing_observation.origin_.z;
          
           if (  !worldToMap3DFloat(  ox,   oy,   oz,   sensor_x,   sensor_y,   sensor_z ) ) {
           RCLCPP_WARN(  
           logger_,  
           "Sensor origin at (  %.2f,   %.2f %.2f ) is out of map bounds (  %.2f,   %.2f,   %.2f ) to (  %.2f,   %.2f,   %.2f ). "
           "The costmap cannot raytrace for it.",  
           ox,   oy,   oz,  
           ox + getSizeInMetersX(   ),   oy + getSizeInMetersY(   ),   oz + getSizeInMetersZ(   ),  
           origin_x_,   origin_y_,   origin_z_ );
          
           return;
           }
          
           bool publish_clearing_points;
          
           {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           publish_clearing_points = (  node->count_subscribers(  "clearing_endpoints" ) > 0 );
           }
          
           clearing_endpoints_->data.clear(   );
           clearing_endpoints_->width = clearing_observation.cloud_->width;
           clearing_endpoints_->height = clearing_observation.cloud_->height;
           clearing_endpoints_->is_dense = true;
           clearing_endpoints_->is_bigendian = false;
          
           sensor_msgs::PointCloud2Modifier modifier(  *clearing_endpoints_ );
           modifier.setPointCloud2Fields(  
           3,   "x",   1,   sensor_msgs::msg::PointField::FLOAT32,  
           "y",   1,   sensor_msgs::msg::PointField::FLOAT32,  
           "z",   1,   sensor_msgs::msg::PointField::FLOAT32 );
          
           sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(  *clearing_endpoints_,   "x" );
           sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(  *clearing_endpoints_,   "y" );
           sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(  *clearing_endpoints_,   "z" );
          
           // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
           double map_end_x = origin_x_ + getSizeInMetersX(   );
           double map_end_y = origin_y_ + getSizeInMetersY(   );
           double map_end_z = origin_z_ + getSizeInMetersZ(   );
          
           sensor_msgs::PointCloud2ConstIterator<float> iter_x(  *(  clearing_observation.cloud_ ),   "x" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_y(  *(  clearing_observation.cloud_ ),   "y" );
           sensor_msgs::PointCloud2ConstIterator<float> iter_z(  *(  clearing_observation.cloud_ ),   "z" );
          
           for (  ; iter_x != iter_x.end(   ); ++iter_x,   ++iter_y,   ++iter_z ) {
           double wpx = *iter_x;
           double wpy = *iter_y;
           double wpz = *iter_z;
          
           double distance = dist(  ox,   oy,   oz,   wpx,   wpy,   wpz );
           double scaling_fact = 1.0;
           scaling_fact = std::max(  std::min(  scaling_fact,   (  distance - 2 * resolution_ ) / distance ),   0.0 );
           wpx = scaling_fact * (  wpx - ox ) + ox;
           wpy = scaling_fact * (  wpy - oy ) + oy;
           wpz = scaling_fact * (  wpz - oz ) + oz;
          
           double a = wpx - ox;
           double b = wpy - oy;
           double c = wpz - oz;
           double t = 1.0;
          
           // we can only raytrace to a maximum z height
           if (  wpz > map_end_z ) {
           // we know we want the vector's z value to be max_z
           t = std::max(  0.0,   std::min(  t,   (  map_end_z - 0.01 - oz ) / c ) );
           } else if (  wpz < origin_z_ ) {
           // and we can only raytrace down to the floor
           // we know we want the vector's z value to be 0.0
           t = std::min(  t,   (  origin_z_ - oz ) / c );
           }
          
           // the minimum value to raytrace from is the origin
           if (  wpx < origin_x_ ) {
           t = std::min(  t,   (  origin_x_ - ox ) / a );
           }
           if (  wpy < origin_y_ ) {
           t = std::min(  t,   (  origin_y_ - oy ) / b );
           }
          
           // the maximum value to raytrace to is the end of the map
           if (  wpx > map_end_x ) {
           t = std::min(  t,   (  map_end_x - ox ) / a );
           }
           if (  wpy > map_end_y ) {
           t = std::min(  t,   (  map_end_y - oy ) / b );
           }
          
           wpx = ox + a * t;
           wpy = oy + b * t;
           wpz = oz + c * t;
          
           double point_x,   point_y,   point_z;
           if (  worldToMap3DFloat(  wpx,   wpy,   wpz,   point_x,   point_y,   point_z ) ) {
           unsigned int cell_raytrace_max_range = cellDistance(  clearing_observation.raytrace_max_range_ );
           unsigned int cell_raytrace_min_range = cellDistance(  clearing_observation.raytrace_min_range_ );
          
          
           // voxel_grid_.markVoxelLine(  sensor_x,   sensor_y,   sensor_z,   point_x,   point_y,   point_z );
           voxel_grid_.clearVoxelLineInMap(  
           sensor_x,   sensor_y,   sensor_z,   point_x,   point_y,   point_z,  
           costmap_,  
           unknown_threshold_,   mark_threshold_,   FREE_SPACE,   NO_INFORMATION,  
           cell_raytrace_max_range,   cell_raytrace_min_range );
          
           updateRaytraceBounds(  
           ox,   oy,   wpx,   wpy,   clearing_observation.raytrace_max_range_,  
           clearing_observation.raytrace_min_range_,   min_x,   min_y,  
           max_x,  
           max_y );
          
           if (  publish_clearing_points ) {
           *clearing_endpoints_iter_x = wpx;
           *clearing_endpoints_iter_y = wpy;
           *clearing_endpoints_iter_z = wpz;
          
           ++clearing_endpoints_iter_x;
           ++clearing_endpoints_iter_y;
           ++clearing_endpoints_iter_z;
           }
           }
           }
          
           if (  publish_clearing_points ) {
           clearing_endpoints_->header.frame_id = global_frame_;
           clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
          
           clearing_endpoints_pub_->publish(  std::move(  clearing_endpoints_ ) );
           }
          }
          
     408  void VoxelLayer::updateOrigin(  double new_origin_x,   double new_origin_y )
          {
           // project the new origin into the grid
           int cell_ox,   cell_oy;
           cell_ox = static_cast<int>(  (  new_origin_x - origin_x_ ) / resolution_ );
           cell_oy = static_cast<int>(  (  new_origin_y - origin_y_ ) / resolution_ );
          
           // compute the associated world coordinates for the origin cell
           // beacuase we want to keep things grid-aligned
           double new_grid_ox,   new_grid_oy;
           new_grid_ox = origin_x_ + cell_ox * resolution_;
           new_grid_oy = origin_y_ + cell_oy * resolution_;
          
           // To save casting from unsigned int to int a bunch of times
           int size_x = size_x_;
           int size_y = size_y_;
          
           // we need to compute the overlap of the new and existing windows
           int lower_left_x,   lower_left_y,   upper_right_x,   upper_right_y;
           lower_left_x = std::min(  std::max(  cell_ox,   0 ),   size_x );
           lower_left_y = std::min(  std::max(  cell_oy,   0 ),   size_y );
           upper_right_x = std::min(  std::max(  cell_ox + size_x,   0 ),   size_x );
           upper_right_y = std::min(  std::max(  cell_oy + size_y,   0 ),   size_y );
          
           unsigned int cell_size_x = upper_right_x - lower_left_x;
           unsigned int cell_size_y = upper_right_y - lower_left_y;
          
           // we need a map to store the obstacles in the window temporarily
           unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
           unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
           unsigned int * voxel_map = voxel_grid_.getData(   );
          
           // copy the local window in the costmap to the local map
           copyMapRegion(  
           costmap_,   lower_left_x,   lower_left_y,   size_x_,   local_map,   0,   0,   cell_size_x,  
           cell_size_x,  
           cell_size_y );
           copyMapRegion(  
           voxel_map,   lower_left_x,   lower_left_y,   size_x_,   local_voxel_map,   0,   0,   cell_size_x,  
           cell_size_x,  
           cell_size_y );
          
           // we'll reset our maps to unknown space if appropriate
           resetMaps(   );
          
           // update the origin with the appropriate world coordinates
           origin_x_ = new_grid_ox;
           origin_y_ = new_grid_oy;
          
           // compute the starting cell location for copying data back in
           int start_x = lower_left_x - cell_ox;
           int start_y = lower_left_y - cell_oy;
          
           // now we want to copy the overlapping information back into the map,   but in its new location
           copyMapRegion(  
           local_map,   0,   0,   cell_size_x,   costmap_,   start_x,   start_y,   size_x_,   cell_size_x,  
           cell_size_y );
           copyMapRegion(  
           local_voxel_map,   0,   0,   cell_size_x,   voxel_map,   start_x,   start_y,   size_x_,  
           cell_size_x,  
           cell_size_y );
          
           // make sure to clean up
           delete[] local_map;
           delete[] local_voxel_map;
          }
          
          /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
          rcl_interfaces::msg::SetParametersResult
     480  VoxelLayer::dynamicParametersCallback(  
     481   std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<Costmap2D::mutex_t> guard(  *getMutex(   ) );
           rcl_interfaces::msg::SetParametersResult result;
           bool resize_map_needed = false;
          
           for (  auto parameter : parameters ) {
           const auto & param_type = parameter.get_type(   );
           const auto & param_name = parameter.get_name(   );
          
           if (  param_type == ParameterType::PARAMETER_DOUBLE ) {
           if (  param_name == name_ + "." + "max_obstacle_height" ) {
           max_obstacle_height_ = parameter.as_double(   );
           } else if (  param_name == name_ + "." + "origin_z" ) {
           origin_z_ = parameter.as_double(   );
           resize_map_needed = true;
           } else if (  param_name == name_ + "." + "z_resolution" ) {
           z_resolution_ = parameter.as_double(   );
           resize_map_needed = true;
           }
           } else if (  param_type == ParameterType::PARAMETER_BOOL ) {
           if (  param_name == name_ + "." + "enabled" ) {
           enabled_ = parameter.as_bool(   );
           current_ = false;
           } else if (  param_name == name_ + "." + "footprint_clearing_enabled" ) {
           footprint_clearing_enabled_ = parameter.as_bool(   );
           } else if (  param_name == name_ + "." + "publish_voxel_map" ) {
           RCLCPP_WARN(  
           logger_,   "publish voxel map is not a dynamic parameter "
           "cannot be changed while running. Rejecting parameter update." );
           continue;
           }
          
           } else if (  param_type == ParameterType::PARAMETER_INTEGER ) {
           if (  param_name == name_ + "." + "z_voxels" ) {
           size_z_ = parameter.as_int(   );
           resize_map_needed = true;
           } else if (  param_name == name_ + "." + "unknown_threshold" ) {
           unknown_threshold_ = parameter.as_int(   ) + (  VOXEL_BITS - size_z_ );
           } else if (  param_name == name_ + "." + "mark_threshold" ) {
           mark_threshold_ = parameter.as_int(   );
           } else if (  param_name == name_ + "." + "combination_method" ) {
           combination_method_ = parameter.as_int(   );
           }
           }
           }
          
           if (  resize_map_needed ) {
           matchSize(   );
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/array_parser.cpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * author: Dave Hershberger
           */
          
          #include <cstdio> // for EOF
          #include <string>
          #include <sstream>
          #include <vector>
          
          namespace nav2_costmap_2d
          {
          
          /** @brief Parse a vector of vector of floats from a string.
           * @param input
           * @param error_return
           * Syntax is [[1.0,   2.0],   [3.3,   4.4,   5.5],   ...] */
      44  std::vector<std::vector<float>> parseVVF(  const std::string & input,   std::string & error_return )
          {
           std::vector<std::vector<float>> result;
          
           std::stringstream input_ss(  input );
           int depth = 0;
           std::vector<float> current_vector;
           while (  !!input_ss && !input_ss.eof(   ) ) {
           switch (  input_ss.peek(   ) ) {
           case EOF:
           break;
           case '[':
           depth++;
           if (  depth > 2 ) {
           error_return = "Array depth greater than 2";
           return result;
           }
           input_ss.get(   );
           current_vector.clear(   );
           break;
           case ']':
           depth--;
           if (  depth < 0 ) {
           error_return = "More close ] than open [";
           return result;
           }
           input_ss.get(   );
           if (  depth == 1 ) {
           result.push_back(  current_vector );
           }
           break;
           case ',  ':
           case ' ':
           case '\t':
           input_ss.get(   );
           break;
           default: // All other characters should be part of the numbers.
           if (  depth != 2 ) {
           std::stringstream err_ss;
           err_ss << "Numbers at depth other than 2. Char was '" << char(  input_ss.peek(   ) ) << "'.";
           error_return = err_ss.str(   );
           return result;
           }
           float value;
           input_ss >> value;
           if (  !!input_ss ) {
           current_vector.push_back(  value );
           }
           break;
           }
           }
          
           if (  depth != 0 ) {
           error_return = "Unterminated vector string.";
           } else {
           error_return = "";
           }
          
           return result;
          }
          
          } // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/clear_costmap_service.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <string>
          #include <algorithm>
          #include <memory>
          
          #include "nav2_costmap_2d/clear_costmap_service.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          namespace nav2_costmap_2d
          {
          
          using std::vector;
          using std::string;
          using std::shared_ptr;
          using std::any_of;
          using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
          using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
          using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
          
      34  ClearCostmapService::ClearCostmapService(  
      35   const nav2_util::LifecycleNode::WeakPtr & parent,  
      36   Costmap2DROS & costmap )
          : costmap_(  costmap )
          {
           auto node = parent.lock(   );
           logger_ = node->get_logger(   );
           reset_value_ = costmap_.getCostmap(   )->getDefaultValue(   );
          
           node->get_parameter(  "clearable_layers",   clearable_layers_ );
          
           clear_except_service_ = node->create_service<ClearExceptRegion>(  
           "clear_except_" + costmap_.getName(   ),  
           std::bind(  
           &ClearCostmapService::clearExceptRegionCallback,   this,  
           std::placeholders::_1,   std::placeholders::_2,   std::placeholders::_3 ) );
          
           clear_around_service_ = node->create_service<ClearAroundRobot>(  
           "clear_around_" + costmap.getName(   ),  
           std::bind(  
           &ClearCostmapService::clearAroundRobotCallback,   this,  
           std::placeholders::_1,   std::placeholders::_2,   std::placeholders::_3 ) );
          
           clear_entire_service_ = node->create_service<ClearEntirely>(  
           "clear_entirely_" + costmap_.getName(   ),  
           std::bind(  
           &ClearCostmapService::clearEntireCallback,   this,  
           std::placeholders::_1,   std::placeholders::_2,   std::placeholders::_3 ) );
          }
          
      64  void ClearCostmapService::clearExceptRegionCallback(  
      65   const shared_ptr<rmw_request_id_t>/*request_header*/,  
      66   const shared_ptr<ClearExceptRegion::Request> request,  
      67   const shared_ptr<ClearExceptRegion::Response>/*response*/ )
          {
           RCLCPP_INFO(  
           logger_,  
           (  "Received request to clear except a region the " + costmap_.getName(   ) ).c_str(   ) );
          
           clearRegion(  request->reset_distance,   true );
          }
          
      76  void ClearCostmapService::clearAroundRobotCallback(  
      77   const shared_ptr<rmw_request_id_t>/*request_header*/,  
      78   const shared_ptr<ClearAroundRobot::Request> request,  
      79   const shared_ptr<ClearAroundRobot::Response>/*response*/ )
          {
           clearRegion(  request->reset_distance,   false );
          }
          
      84  void ClearCostmapService::clearEntireCallback(  
      85   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
      86   const std::shared_ptr<ClearEntirely::Request>/*request*/,  
      87   const std::shared_ptr<ClearEntirely::Response>/*response*/ )
          {
           RCLCPP_INFO(  
           logger_,  
           (  "Received request to clear entirely the " + costmap_.getName(   ) ).c_str(   ) );
          
           clearEntirely(   );
          }
          
      96  void ClearCostmapService::clearRegion(  const double reset_distance,   bool invert )
          {
           double x,   y;
          
           if (  !getPosition(  x,   y ) ) {
           RCLCPP_ERROR(  
           logger_,  
           "Cannot clear map because robot pose cannot be retrieved." );
           return;
           }
          
           auto layers = costmap_.getLayeredCostmap(   )->getPlugins(   );
          
           for (  auto & layer : *layers ) {
           if (  layer->isClearable(   ) ) {
           auto costmap_layer = std::static_pointer_cast<CostmapLayer>(  layer );
           clearLayerRegion(  costmap_layer,   x,   y,   reset_distance,   invert );
           }
           }
          
           // AlexeyMerzlyakov: No need to clear layer region for costmap filters
           // as they are always supposed to be not clearable.
          }
          
     120  void ClearCostmapService::clearLayerRegion(  
     121   shared_ptr<CostmapLayer> & costmap,   double pose_x,   double pose_y,   double reset_distance,  
     122   bool invert )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  costmap->getMutex(   ) ) );
          
           double start_point_x = pose_x - reset_distance / 2;
           double start_point_y = pose_y - reset_distance / 2;
           double end_point_x = start_point_x + reset_distance;
           double end_point_y = start_point_y + reset_distance;
          
           int start_x,   start_y,   end_x,   end_y;
           costmap->worldToMapEnforceBounds(  start_point_x,   start_point_y,   start_x,   start_y );
           costmap->worldToMapEnforceBounds(  end_point_x,   end_point_y,   end_x,   end_y );
          
           costmap->clearArea(  start_x,   start_y,   end_x,   end_y,   invert );
          
           double ox = costmap->getOriginX(   ),   oy = costmap->getOriginY(   );
           double width = costmap->getSizeInMetersX(   ),   height = costmap->getSizeInMetersY(   );
           costmap->addExtraBounds(  ox,   oy,   ox + width,   oy + height );
          }
          
     142  void ClearCostmapService::clearEntirely(   )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  costmap_.getCostmap(   )->getMutex(   ) ) );
           costmap_.resetLayers(   );
          }
          
     148  bool ClearCostmapService::getPosition(  double & x,   double & y ) const
          {
           geometry_msgs::msg::PoseStamped pose;
           if (  !costmap_.getRobotPose(  pose ) ) {
           return false;
           }
          
           x = pose.pose.position.x;
           y = pose.pose.position.y;
          
           return true;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #include "nav2_costmap_2d/costmap_2d.hpp"
          
          #include <algorithm>
          #include <cstdio>
          #include <string>
          #include <vector>
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_util/occ_grid_values.hpp"
          
          namespace nav2_costmap_2d
          {
      49  Costmap2D::Costmap2D(  
           unsigned int cells_size_x,   unsigned int cells_size_y,   double resolution,  
           double origin_x,   double origin_y,   unsigned char default_value )
          : size_x_(  cells_size_x ),   size_y_(  cells_size_y ),   resolution_(  resolution ),   origin_x_(  origin_x ),  
           origin_y_(  origin_y ),   costmap_(  NULL ),   default_value_(  default_value )
          {
           access_ = new mutex_t(   );
          
           // create the costmap
           initMaps(  size_x_,   size_y_ );
           resetMaps(   );
          }
          
      62  Costmap2D::Costmap2D(  const nav_msgs::msg::OccupancyGrid & map )
          : default_value_(  FREE_SPACE )
          {
           access_ = new mutex_t(   );
          
           // fill local variables
           size_x_ = map.info.width;
           size_y_ = map.info.height;
           resolution_ = map.info.resolution;
           origin_x_ = map.info.origin.position.x;
           origin_y_ = map.info.origin.position.y;
          
           // create the costmap
           costmap_ = new unsigned char[size_x_ * size_y_];
          
           // fill the costmap with a data
           int8_t data;
           for (  unsigned int it = 0; it < size_x_ * size_y_; it++ ) {
           data = map.data[it];
           if (  data == nav2_util::OCC_GRID_UNKNOWN ) {
           costmap_[it] = NO_INFORMATION;
           } else {
           // Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED]
           // to costmap data range [FREE_SPACE..LETHAL_OBSTACLE]
           costmap_[it] = std::round(  
           static_cast<double>(  data ) * (  LETHAL_OBSTACLE - FREE_SPACE ) /
           (  nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE ) );
           }
           }
          }
          
      93  void Costmap2D::deleteMaps(   )
          {
           // clean up data
           std::unique_lock<mutex_t> lock(  *access_ );
           delete[] costmap_;
           costmap_ = NULL;
          }
          
     101  void Costmap2D::initMaps(  unsigned int size_x,   unsigned int size_y )
          {
           std::unique_lock<mutex_t> lock(  *access_ );
           delete[] costmap_;
           costmap_ = new unsigned char[size_x * size_y];
          }
          
     108  void Costmap2D::resizeMap(  
           unsigned int size_x,   unsigned int size_y,   double resolution,  
           double origin_x,   double origin_y )
          {
           size_x_ = size_x;
           size_y_ = size_y;
           resolution_ = resolution;
           origin_x_ = origin_x;
           origin_y_ = origin_y;
          
           initMaps(  size_x,   size_y );
          
           // reset our maps to have no information
           resetMaps(   );
          }
          
     124  void Costmap2D::resetMaps(   )
          {
           std::unique_lock<mutex_t> lock(  *access_ );
           memset(  costmap_,   default_value_,   size_x_ * size_y_ * sizeof(  unsigned char ) );
          }
          
     130  void Costmap2D::resetMap(  unsigned int x0,   unsigned int y0,   unsigned int xn,   unsigned int yn )
          {
           resetMapToValue(  x0,   y0,   xn,   yn,   default_value_ );
          }
          
     135  void Costmap2D::resetMapToValue(  
           unsigned int x0,   unsigned int y0,   unsigned int xn,   unsigned int yn,   unsigned char value )
          {
           std::unique_lock<mutex_t> lock(  *(  access_ ) );
           unsigned int len = xn - x0;
           for (  unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_ ) {
           memset(  costmap_ + y,   value,   len * sizeof(  unsigned char ) );
           }
          }
          
     145  bool Costmap2D::copyCostmapWindow(  
           const Costmap2D & map,   double win_origin_x,   double win_origin_y,  
           double win_size_x,  
           double win_size_y )
          {
           // check for self windowing
           if (  this == &map ) {
           // ROS_ERROR(  "Cannot convert this costmap into a window of itself" );
           return false;
           }
          
           // clean up old data
           deleteMaps(   );
          
           // compute the bounds of our new map
           unsigned int lower_left_x,   lower_left_y,   upper_right_x,   upper_right_y;
           if (  !map.worldToMap(  win_origin_x,   win_origin_y,   lower_left_x,   lower_left_y ) ||
           !map.worldToMap(  
           win_origin_x + win_size_x,   win_origin_y + win_size_y,   upper_right_x,  
           upper_right_y ) )
           {
           // ROS_ERROR(  "Cannot window a map that the window bounds don't fit inside of" );
           return false;
           }
          
           size_x_ = upper_right_x - lower_left_x;
           size_y_ = upper_right_y - lower_left_y;
           resolution_ = map.resolution_;
           origin_x_ = win_origin_x;
           origin_y_ = win_origin_y;
          
           // initialize our various maps and reset markers for inflation
           initMaps(  size_x_,   size_y_ );
          
           // copy the window of the static map and the costmap that we're taking
           copyMapRegion(  
           map.costmap_,   lower_left_x,   lower_left_y,   map.size_x_,   costmap_,   0,   0,   size_x_,  
           size_x_,  
           size_y_ );
           return true;
          }
          
     187  bool Costmap2D::copyWindow(  
           const Costmap2D & source,  
           unsigned int sx0,   unsigned int sy0,   unsigned int sxn,   unsigned int syn,  
           unsigned int dx0,   unsigned int dy0 )
          {
           const unsigned int sz_x = sxn - sx0;
           const unsigned int sz_y = syn - sy0;
          
           if (  sxn > source.getSizeInCellsX(   ) || syn > source.getSizeInCellsY(   ) ) {
           return false;
           }
          
           if (  dx0 + sz_x > size_x_ || dy0 + sz_y > size_y_ ) {
           return false;
           }
          
           copyMapRegion(  
           source.costmap_,   sx0,   sy0,   source.size_x_,  
           costmap_,   dx0,   dy0,   size_x_,  
           sz_x,   sz_y );
           return true;
          }
          
     210  Costmap2D & Costmap2D::operator=(  const Costmap2D & map )
          {
           // check for self assignement
           if (  this == &map ) {
           return *this;
           }
          
           // clean up old data
           deleteMaps(   );
          
           size_x_ = map.size_x_;
           size_y_ = map.size_y_;
           resolution_ = map.resolution_;
           origin_x_ = map.origin_x_;
           origin_y_ = map.origin_y_;
          
           // initialize our various maps
           initMaps(  size_x_,   size_y_ );
          
           // copy the cost map
           memcpy(  costmap_,   map.costmap_,   size_x_ * size_y_ * sizeof(  unsigned char ) );
          
           return *this;
          }
          
     235  Costmap2D::Costmap2D(  const Costmap2D & map )
          : costmap_(  NULL )
          {
           access_ = new mutex_t(   );
           *this = map;
          }
          
          // just initialize everything to NULL by default
     243  Costmap2D::Costmap2D(   )
          : size_x_(  0 ),   size_y_(  0 ),   resolution_(  0.0 ),   origin_x_(  0.0 ),   origin_y_(  0.0 ),   costmap_(  NULL )
          {
           access_ = new mutex_t(   );
          }
          
     249  Costmap2D::~Costmap2D(   )
          {
           deleteMaps(   );
           delete access_;
          }
          
     255  unsigned int Costmap2D::cellDistance(  double world_dist )
          {
           double cells_dist = std::max(  0.0,   ceil(  world_dist / resolution_ ) );
           return (  unsigned int )cells_dist;
          }
          
     261  unsigned char * Costmap2D::getCharMap(   ) const
          {
           return costmap_;
          }
          
     266  unsigned char Costmap2D::getCost(  unsigned int mx,   unsigned int my ) const
          {
           return costmap_[getIndex(  mx,   my )];
          }
          
     271  unsigned char Costmap2D::getCost(  unsigned int undex ) const
          {
           return costmap_[undex];
          }
          
     276  void Costmap2D::setCost(  unsigned int mx,   unsigned int my,   unsigned char cost )
          {
           costmap_[getIndex(  mx,   my )] = cost;
          }
          
     281  void Costmap2D::mapToWorld(  unsigned int mx,   unsigned int my,   double & wx,   double & wy ) const
          {
           wx = origin_x_ + (  mx + 0.5 ) * resolution_;
           wy = origin_y_ + (  my + 0.5 ) * resolution_;
          }
          
     287  bool Costmap2D::worldToMap(  double wx,   double wy,   unsigned int & mx,   unsigned int & my ) const
          {
           if (  wx < origin_x_ || wy < origin_y_ ) {
           return false;
           }
          
           mx = static_cast<unsigned int>(  (  wx - origin_x_ ) / resolution_ );
           my = static_cast<unsigned int>(  (  wy - origin_y_ ) / resolution_ );
          
           if (  mx < size_x_ && my < size_y_ ) {
           return true;
           }
           return false;
          }
          
     302  void Costmap2D::worldToMapNoBounds(  double wx,   double wy,   int & mx,   int & my ) const
          {
           mx = static_cast<int>(  (  wx - origin_x_ ) / resolution_ );
           my = static_cast<int>(  (  wy - origin_y_ ) / resolution_ );
          }
          
     308  void Costmap2D::worldToMapEnforceBounds(  double wx,   double wy,   int & mx,   int & my ) const
          {
           // Here we avoid doing any math to wx,  wy before comparing them to
           // the bounds,   so their values can go out to the max and min values
           // of double floating point.
           if (  wx < origin_x_ ) {
           mx = 0;
           } else if (  wx > resolution_ * size_x_ + origin_x_ ) {
           mx = size_x_ - 1;
           } else {
           mx = static_cast<int>(  (  wx - origin_x_ ) / resolution_ );
           }
          
           if (  wy < origin_y_ ) {
           my = 0;
           } else if (  wy > resolution_ * size_y_ + origin_y_ ) {
           my = size_y_ - 1;
           } else {
           my = static_cast<int>(  (  wy - origin_y_ ) / resolution_ );
           }
          }
          
     330  void Costmap2D::updateOrigin(  double new_origin_x,   double new_origin_y )
          {
           // project the new origin into the grid
           int cell_ox,   cell_oy;
           cell_ox = static_cast<int>(  (  new_origin_x - origin_x_ ) / resolution_ );
           cell_oy = static_cast<int>(  (  new_origin_y - origin_y_ ) / resolution_ );
          
           // compute the associated world coordinates for the origin cell
           // because we want to keep things grid-aligned
           double new_grid_ox,   new_grid_oy;
           new_grid_ox = origin_x_ + cell_ox * resolution_;
           new_grid_oy = origin_y_ + cell_oy * resolution_;
          
           // To save casting from unsigned int to int a bunch of times
           int size_x = size_x_;
           int size_y = size_y_;
          
           // we need to compute the overlap of the new and existing windows
           int lower_left_x,   lower_left_y,   upper_right_x,   upper_right_y;
           lower_left_x = std::min(  std::max(  cell_ox,   0 ),   size_x );
           lower_left_y = std::min(  std::max(  cell_oy,   0 ),   size_y );
           upper_right_x = std::min(  std::max(  cell_ox + size_x,   0 ),   size_x );
           upper_right_y = std::min(  std::max(  cell_oy + size_y,   0 ),   size_y );
          
           unsigned int cell_size_x = upper_right_x - lower_left_x;
           unsigned int cell_size_y = upper_right_y - lower_left_y;
          
           // we need a map to store the obstacles in the window temporarily
           unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
          
           // copy the local window in the costmap to the local map
           copyMapRegion(  
           costmap_,   lower_left_x,   lower_left_y,   size_x_,   local_map,   0,   0,   cell_size_x,  
           cell_size_x,  
           cell_size_y );
          
           // now we'll set the costmap to be completely unknown if we track unknown space
           resetMaps(   );
          
           // update the origin with the appropriate world coordinates
           origin_x_ = new_grid_ox;
           origin_y_ = new_grid_oy;
          
           // compute the starting cell location for copying data back in
           int start_x = lower_left_x - cell_ox;
           int start_y = lower_left_y - cell_oy;
          
           // now we want to copy the overlapping information back into the map,   but in its new location
           copyMapRegion(  
           local_map,   0,   0,   cell_size_x,   costmap_,   start_x,   start_y,   size_x_,   cell_size_x,  
           cell_size_y );
          
           // make sure to clean up
           delete[] local_map;
          }
          
     386  bool Costmap2D::setConvexPolygonCost(  
     387   const std::vector<geometry_msgs::msg::Point> & polygon,  
           unsigned char cost_value )
          {
           // we assume the polygon is given in the global_frame...
           // we need to transform it to map coordinates
           std::vector<MapLocation> map_polygon;
           for (  unsigned int i = 0; i < polygon.size(   ); ++i ) {
           MapLocation loc;
           if (  !worldToMap(  polygon[i].x,   polygon[i].y,   loc.x,   loc.y ) ) {
           // (  "Polygon lies outside map bounds,   so we can't fill it" );
           return false;
           }
           map_polygon.push_back(  loc );
           }
          
           std::vector<MapLocation> polygon_cells;
          
           // get the cells that fill the polygon
           convexFillCells(  map_polygon,   polygon_cells );
          
           // set the cost of those cells
           for (  unsigned int i = 0; i < polygon_cells.size(   ); ++i ) {
           unsigned int index = getIndex(  polygon_cells[i].x,   polygon_cells[i].y );
           costmap_[index] = cost_value;
           }
           return true;
          }
          
     415  void Costmap2D::polygonOutlineCells(  
     416   const std::vector<MapLocation> & polygon,  
     417   std::vector<MapLocation> & polygon_cells )
          {
           PolygonOutlineCells cell_gatherer(  *this,   costmap_,   polygon_cells );
           for (  unsigned int i = 0; i < polygon.size(   ) - 1; ++i ) {
           raytraceLine(  cell_gatherer,   polygon[i].x,   polygon[i].y,   polygon[i + 1].x,   polygon[i + 1].y );
           }
           if (  !polygon.empty(   ) ) {
           unsigned int last_index = polygon.size(   ) - 1;
           // we also need to close the polygon by going from the last point to the first
           raytraceLine(  
           cell_gatherer,   polygon[last_index].x,   polygon[last_index].y,   polygon[0].x,  
           polygon[0].y );
           }
          }
          
     432  void Costmap2D::convexFillCells(  
     433   const std::vector<MapLocation> & polygon,  
     434   std::vector<MapLocation> & polygon_cells )
          {
           // we need a minimum polygon of a triangle
           if (  polygon.size(   ) < 3 ) {
           return;
           }
          
           // first get the cells that make up the outline of the polygon
           polygonOutlineCells(  polygon,   polygon_cells );
          
           // quick bubble sort to sort points by x
           MapLocation swap;
           unsigned int i = 0;
           while (  i < polygon_cells.size(   ) - 1 ) {
           if (  polygon_cells[i].x > polygon_cells[i + 1].x ) {
           swap = polygon_cells[i];
           polygon_cells[i] = polygon_cells[i + 1];
           polygon_cells[i + 1] = swap;
          
           if (  i > 0 ) {
           --i;
           }
           } else {
           ++i;
           }
           }
          
           i = 0;
           MapLocation min_pt;
           MapLocation max_pt;
           unsigned int min_x = polygon_cells[0].x;
           unsigned int max_x = polygon_cells[polygon_cells.size(   ) - 1].x;
          
           // walk through each column and mark cells inside the polygon
           for (  unsigned int x = min_x; x <= max_x; ++x ) {
           if (  i >= polygon_cells.size(   ) - 1 ) {
           break;
           }
          
           if (  polygon_cells[i].y < polygon_cells[i + 1].y ) {
           min_pt = polygon_cells[i];
           max_pt = polygon_cells[i + 1];
           } else {
           min_pt = polygon_cells[i + 1];
           max_pt = polygon_cells[i];
           }
          
           i += 2;
           while (  i < polygon_cells.size(   ) && polygon_cells[i].x == x ) {
           if (  polygon_cells[i].y < min_pt.y ) {
           min_pt = polygon_cells[i];
           } else if (  polygon_cells[i].y > max_pt.y ) {
           max_pt = polygon_cells[i];
           }
           ++i;
           }
          
           MapLocation pt;
           // loop though cells in the column
           for (  unsigned int y = min_pt.y; y <= max_pt.y; ++y ) {
           pt.x = x;
           pt.y = y;
           polygon_cells.push_back(  pt );
           }
           }
          }
          
     501  unsigned int Costmap2D::getSizeInCellsX(   ) const
          {
           return size_x_;
          }
          
     506  unsigned int Costmap2D::getSizeInCellsY(   ) const
          {
           return size_y_;
          }
          
     511  double Costmap2D::getSizeInMetersX(   ) const
          {
           return (  size_x_ - 1 + 0.5 ) * resolution_;
          }
          
     516  double Costmap2D::getSizeInMetersY(   ) const
          {
           return (  size_y_ - 1 + 0.5 ) * resolution_;
          }
          
     521  double Costmap2D::getOriginX(   ) const
          {
           return origin_x_;
          }
          
     526  double Costmap2D::getOriginY(   ) const
          {
           return origin_y_;
          }
          
     531  double Costmap2D::getResolution(   ) const
          {
           return resolution_;
          }
          
     536  bool Costmap2D::saveMap(  std::string file_name )
          {
           FILE * fp = fopen(  file_name.c_str(   ),   "w" );
          
           if (  !fp ) {
           return false;
           }
          
           fprintf(  fp,   "P2\n%u\n%u\n%u\n",   size_x_,   size_y_,   0xff );
           for (  unsigned int iy = 0; iy < size_y_; iy++ ) {
           for (  unsigned int ix = 0; ix < size_x_; ix++ ) {
           unsigned char cost = getCost(  ix,   iy );
           fprintf(  fp,   "%d ",   cost );
           }
           fprintf(  fp,   "\n" );
           }
           fclose(  fp );
           return true;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d_cloud.cpp

       1  /*
           * Copyright (  C ) 2009,   Willow Garage,   Inc.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           * * Redistributions of source code must retain the above copyright notice,  
           * this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the names of Stanford University or Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <utility>
          
          #include "rclcpp/rclcpp.hpp"
          #include "sensor_msgs/msg/point_cloud2.hpp"
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          #include "nav2_voxel_grid/voxel_grid.hpp"
          #include "nav2_msgs/msg/voxel_grid.hpp"
          #include "nav2_util/execution_timer.hpp"
          
      40  static inline void mapToWorld3D(  
           const unsigned int mx,  
           const unsigned int my,   const unsigned int mz,  
           const double origin_x,   const double origin_y,   const double origin_z,  
           const double x_resolution,   const double y_resolution,  
           const double z_resolution,  
           double & wx,   double & wy,   double & wz )
          {
           // returns the center point of the cell
           wx = origin_x + (  mx + 0.5 ) * x_resolution;
           wy = origin_y + (  my + 0.5 ) * y_resolution;
           wz = origin_z + (  mz + 0.5 ) * z_resolution;
          }
          
          struct Cell
          {
           double x;
           double y;
           double z;
           nav2_voxel_grid::VoxelStatus status;
          };
          typedef std::vector<Cell> V_Cell;
          
          float g_colors_r[] = {0.0f,   0.0f,   1.0f};
          float g_colors_g[] = {0.0f,   0.0f,   0.0f};
          float g_colors_b[] = {0.0f,   1.0f,   0.0f};
          float g_colors_a[] = {0.0f,   0.5f,   1.0f};
          
          V_Cell g_marked;
          V_Cell g_unknown;
          
          rclcpp::Node::SharedPtr g_node;
          
          rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
          rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
          
          /**
           * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
           * @param cloud PointCloud2 Ptr which needs to be filled
           * @param num_channels Represents the total number of points that are going to be filled
           * @param header Carries the header information that needs to be assigned to PointCloud2 header
           * @param g_cells contains the x,   y,   z values that needs to be added to the PointCloud2
           */
      83  void pointCloud2Helper(  
           std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,  
           uint32_t num_channels,  
           std_msgs::msg::Header header,  
           V_Cell & g_cells )
          {
           cloud->header = header;
           cloud->width = num_channels;
           cloud->height = 1;
           cloud->is_dense = true;
           cloud->is_bigendian = false;
           sensor_msgs::PointCloud2Modifier modifier(  *cloud );
          
           modifier.setPointCloud2Fields(  
           6,   "x",   1,   sensor_msgs::msg::PointField::FLOAT32,  
           "y",   1,   sensor_msgs::msg::PointField::FLOAT32,  
           "z",   1,   sensor_msgs::msg::PointField::FLOAT32,  
           "r",   1,   sensor_msgs::msg::PointField::UINT8,  
           "g",   1,   sensor_msgs::msg::PointField::UINT8,  
           "b",   1,   sensor_msgs::msg::PointField::UINT8 );
          
           sensor_msgs::PointCloud2Iterator<float> iter_x(  *cloud,   "x" );
           sensor_msgs::PointCloud2Iterator<float> iter_y(  *cloud,   "y" );
           sensor_msgs::PointCloud2Iterator<float> iter_z(  *cloud,   "z" );
          
           sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(  *cloud,   "r" );
           sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(  *cloud,   "g" );
           sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(  *cloud,   "b" );
          
           for (  uint32_t i = 0; i < num_channels; ++i ) {
           Cell & c = g_cells[i];
           // assigning value to the point cloud2's iterator
           *iter_x = c.x;
           *iter_y = c.y;
           *iter_z = c.z;
           *iter_r = g_colors_r[c.status] * 255.0;
           *iter_g = g_colors_g[c.status] * 255.0;
           *iter_b = g_colors_b[c.status] * 255.0;
          
           ++iter_x;
           ++iter_y;
           ++iter_z;
           ++iter_r;
           ++iter_g;
           ++iter_b;
           }
          }
          
     131  void voxelCallback(  const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid )
          {
           if (  grid->data.empty(   ) ) {
           RCLCPP_ERROR(  g_node->get_logger(   ),   "Received empty voxel grid" );
           return;
           }
          
           nav2_util::ExecutionTimer timer;
           timer.start(   );
          
           RCLCPP_DEBUG(  g_node->get_logger(   ),   "Received voxel grid" );
           const std::string frame_id = grid->header.frame_id;
           const rclcpp::Time stamp = grid->header.stamp;
           const uint32_t * data = &grid->data.front(   );
           const double x_origin = grid->origin.x;
           const double y_origin = grid->origin.y;
           const double z_origin = grid->origin.z;
           const double x_res = grid->resolutions.x;
           const double y_res = grid->resolutions.y;
           const double z_res = grid->resolutions.z;
           const uint32_t x_size = grid->size_x;
           const uint32_t y_size = grid->size_y;
           const uint32_t z_size = grid->size_z;
          
           g_marked.clear(   );
           g_unknown.clear(   );
           uint32_t num_marked = 0;
           uint32_t num_unknown = 0;
           for (  uint32_t y_grid = 0; y_grid < y_size; ++y_grid ) {
           for (  uint32_t x_grid = 0; x_grid < x_size; ++x_grid ) {
           for (  uint32_t z_grid = 0; z_grid < z_size; ++z_grid ) {
           nav2_voxel_grid::VoxelStatus status =
           nav2_voxel_grid::VoxelGrid::getVoxel(  
           x_grid,   y_grid,  
           z_grid,   x_size,   y_size,   z_size,   data );
           if (  status == nav2_voxel_grid::UNKNOWN ) {
           Cell c;
           c.status = status;
           mapToWorld3D(  
           x_grid,   y_grid,   z_grid,   x_origin,   y_origin,  
           z_origin,   x_res,   y_res,   z_res,   c.x,   c.y,   c.z );
          
           g_unknown.push_back(  c );
          
           ++num_unknown;
           } else if (  status == nav2_voxel_grid::MARKED ) {
           Cell c;
           c.status = status;
           mapToWorld3D(  
           x_grid,   y_grid,   z_grid,   x_origin,   y_origin,  
           z_origin,   x_res,   y_res,   z_res,   c.x,   c.y,   c.z );
          
           g_marked.push_back(  c );
          
           ++num_marked;
           }
           }
           }
           }
          
           std_msgs::msg::Header pcl_header;
           pcl_header.frame_id = frame_id;
           pcl_header.stamp = stamp;
          
           {
           auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>(   );
           pointCloud2Helper(  cloud,   num_marked,   pcl_header,   g_marked );
           pub_marked->publish(  std::move(  cloud ) );
           }
          
           {
           auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>(   );
           pointCloud2Helper(  cloud,   num_unknown,   pcl_header,   g_unknown );
           pub_unknown->publish(  std::move(  cloud ) );
           }
          
           timer.end(   );
           RCLCPP_DEBUG(  
           g_node->get_logger(   ),   "Published %d points in %f seconds",  
           num_marked + num_unknown,   timer.elapsed_time_in_seconds(   ) );
          }
          
     213  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           g_node = rclcpp::Node::make_shared(  "costmap_2d_cloud" );
          
           RCLCPP_DEBUG(  g_node->get_logger(   ),   "Starting up costmap_2d_cloud" );
          
           pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(  
           "voxel_marked_cloud",   1 );
           pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(  
           "voxel_unknown_cloud",   1 );
           auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(  
           "voxel_grid",   rclcpp::SystemDefaultsQoS(   ),   voxelCallback );
          
           rclcpp::spin(  g_node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_costmap_2d/src/costmap_2d_markers.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           * Steve Macenski
           *********************************************************************/
          #include <string>
          #include <vector>
          #include <memory>
          #include <utility>
          
          #include "rclcpp/rclcpp.hpp"
          #include "visualization_msgs/msg/marker.hpp"
          #include "nav2_msgs/msg/voxel_grid.hpp"
          #include "nav2_voxel_grid/voxel_grid.hpp"
          #include "nav2_util/execution_timer.hpp"
          
          struct Cell
          {
           double x;
           double y;
           double z;
           nav2_voxel_grid::VoxelStatus status;
          };
          typedef std::vector<Cell> V_Cell;
          
          float g_colors_r[] = {0.0f,   0.0f,   1.0f};
          float g_colors_g[] = {0.0f,   0.0f,   0.0f};
          float g_colors_b[] = {0.0f,   1.0f,   0.0f};
          float g_colors_a[] = {0.0f,   0.5f,   1.0f};
          
          V_Cell g_cells;
          rclcpp::Node::SharedPtr g_node;
          rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub;
          
      68  void voxelCallback(  const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid )
          {
           if (  grid->data.empty(   ) ) {
           RCLCPP_ERROR(  g_node->get_logger(   ),   "Received voxel grid" );
           return;
           }
          
           nav2_util::ExecutionTimer timer;
           timer.start(   );
          
           RCLCPP_DEBUG(  g_node->get_logger(   ),   "Received voxel grid" );
          
           const std::string frame_id = grid->header.frame_id;
           const rclcpp::Time stamp = grid->header.stamp;
           const uint32_t * data = &grid->data.front(   );
           const double x_origin = grid->origin.x;
           const double y_origin = grid->origin.y;
           const double z_origin = grid->origin.z;
           const double x_res = grid->resolutions.x;
           const double y_res = grid->resolutions.y;
           const double z_res = grid->resolutions.z;
           const uint32_t x_size = grid->size_x;
           const uint32_t y_size = grid->size_y;
           const uint32_t z_size = grid->size_z;
          
           g_cells.clear(   );
           uint32_t num_markers = 0;
           for (  uint32_t y_grid = 0; y_grid < y_size; ++y_grid ) {
           for (  uint32_t x_grid = 0; x_grid < x_size; ++x_grid ) {
           for (  uint32_t z_grid = 0; z_grid < z_size; ++z_grid ) {
           nav2_voxel_grid::VoxelStatus status =
           nav2_voxel_grid::VoxelGrid::getVoxel(  
           x_grid,   y_grid,  
           z_grid,   x_size,   y_size,   z_size,   data );
           if (  status == nav2_voxel_grid::MARKED ) {
           Cell c;
           c.status = status;
           c.x = x_origin + (  x_grid + 0.5 ) * x_res;
           c.y = y_origin + (  y_grid + 0.5 ) * y_res;
           c.z = z_origin + (  z_grid + 0.5 ) * z_res;
           g_cells.push_back(  c );
          
           ++num_markers;
           }
           }
           }
           }
          
           auto m = std::make_unique<visualization_msgs::msg::Marker>(   );
           m->header.frame_id = frame_id;
           m->header.stamp = stamp;
           m->ns = g_node->get_namespace(   );
           m->id = 0;
           m->type = visualization_msgs::msg::Marker::CUBE_LIST;
           m->action = visualization_msgs::msg::Marker::ADD;
           m->pose.orientation.w = 1.0;
           m->scale.x = x_res;
           m->scale.y = y_res;
           m->scale.z = z_res;
           m->color.r = g_colors_r[nav2_voxel_grid::MARKED];
           m->color.g = g_colors_g[nav2_voxel_grid::MARKED];
           m->color.b = g_colors_b[nav2_voxel_grid::MARKED];
           m->color.a = g_colors_a[nav2_voxel_grid::MARKED];
           m->points.resize(  num_markers );
           for (  uint32_t i = 0; i < num_markers; ++i ) {
           Cell & c = g_cells[i];
           geometry_msgs::msg::Point & p = m->points[i];
           p.x = c.x;
           p.y = c.y;
           p.z = c.z;
           }
          
           pub->publish(  std::move(  m ) );
          
           timer.end(   );
           RCLCPP_INFO(  
           g_node->get_logger(   ),   "Published %d markers in %f seconds",  
           num_markers,   timer.elapsed_time_in_seconds(   ) );
          }
          
     148  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           g_node = rclcpp::Node::make_shared(  "costmap_2d_marker" );
          
           RCLCPP_DEBUG(  g_node->get_logger(   ),   "Starting costmap_2d_marker" );
          
           pub = g_node->create_publisher<visualization_msgs::msg::Marker>(  
           "visualization_marker",   1 );
          
           auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(  
           "voxel_grid",   rclcpp::SystemDefaultsQoS(   ),   voxelCallback );
          
           rclcpp::spin(  g_node->get_node_base_interface(   ) );
          }

navigation2/nav2_costmap_2d/src/costmap_2d_node.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #include <memory>
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      43  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "costmap" );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_costmap_2d/src/costmap_2d_publisher.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * Copyright (  c ) 2019,   Samsung Research America,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
          
          #include <string>
          #include <memory>
          #include <utility>
          
          #include "nav2_costmap_2d/cost_values.hpp"
          
          namespace nav2_costmap_2d
          {
          
          char * Costmap2DPublisher::cost_translation_table_ = NULL;
          
      52  Costmap2DPublisher::Costmap2DPublisher(  
      53   const nav2_util::LifecycleNode::WeakPtr & parent,  
      54   Costmap2D * costmap,  
      55   std::string global_frame,  
      56   std::string topic_name,  
      57   bool always_send_full_costmap )
          : costmap_(  costmap ),  
           global_frame_(  global_frame ),  
           topic_name_(  topic_name ),  
           active_(  false ),  
           always_send_full_costmap_(  always_send_full_costmap )
          {
           auto node = parent.lock(   );
           clock_ = node->get_clock(   );
           logger_ = node->get_logger(   );
          
           auto custom_qos = rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   );
          
           // TODO(  bpwilcox ): port onNewSubscription functionality for publisher
           costmap_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(  
           topic_name,  
           custom_qos );
           costmap_raw_pub_ = node->create_publisher<nav2_msgs::msg::Costmap>(  
           topic_name + "_raw",  
           custom_qos );
           costmap_update_pub_ = node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(  
           topic_name + "_updates",   custom_qos );
          
           // Create a service that will use the callback function to handle requests.
           costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(  
           "get_costmap",   std::bind(  
           &Costmap2DPublisher::costmap_service_callback,  
           this,   std::placeholders::_1,   std::placeholders::_2,  
           std::placeholders::_3 ) );
          
           if (  cost_translation_table_ == NULL ) {
           cost_translation_table_ = new char[256];
          
           // special values:
           cost_translation_table_[0] = 0; // NO obstacle
           cost_translation_table_[253] = 99; // INSCRIBED obstacle
           cost_translation_table_[254] = 100; // LETHAL obstacle
           cost_translation_table_[255] = -1; // UNKNOWN
          
           // regular cost values scale the range 1 to 252 (  inclusive ) to fit
           // into 1 to 98 (  inclusive ).
           for (  int i = 1; i < 253; i++ ) {
           cost_translation_table_[i] = static_cast<char>(  1 + (  97 * (  i - 1 ) ) / 251 );
           }
           }
          
           xn_ = yn_ = 0;
           x0_ = costmap_->getSizeInCellsX(   );
           y0_ = costmap_->getSizeInCellsY(   );
          }
          
     108  Costmap2DPublisher::~Costmap2DPublisher(   ) {}
          
          // TODO(  bpwilcox ): find equivalent/workaround to ros::SingleSubscriberPublishr
          /*
          void Costmap2DPublisher::onNewSubscription(  const ros::SingleSubscriberPublisher& pub )
          {
           prepareGrid(   );
           pub.publish(  grid_ );
          } */
          
          // prepare grid_ message for publication.
     119  void Costmap2DPublisher::prepareGrid(   )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  costmap_->getMutex(   ) ) );
           grid_resolution = costmap_->getResolution(   );
           grid_width = costmap_->getSizeInCellsX(   );
           grid_height = costmap_->getSizeInCellsY(   );
          
           grid_ = std::make_unique<nav_msgs::msg::OccupancyGrid>(   );
          
           grid_->header.frame_id = global_frame_;
           grid_->header.stamp = clock_->now(   );
          
           grid_->info.resolution = grid_resolution;
          
           grid_->info.width = grid_width;
           grid_->info.height = grid_height;
          
           double wx,   wy;
           costmap_->mapToWorld(  0,   0,   wx,   wy );
           grid_->info.origin.position.x = wx - grid_resolution / 2;
           grid_->info.origin.position.y = wy - grid_resolution / 2;
           grid_->info.origin.position.z = 0.0;
           grid_->info.origin.orientation.w = 1.0;
           saved_origin_x_ = costmap_->getOriginX(   );
           saved_origin_y_ = costmap_->getOriginY(   );
          
           grid_->data.resize(  grid_->info.width * grid_->info.height );
          
           unsigned char * data = costmap_->getCharMap(   );
           for (  unsigned int i = 0; i < grid_->data.size(   ); i++ ) {
           grid_->data[i] = cost_translation_table_[data[i]];
           }
          }
          
     153  void Costmap2DPublisher::prepareCostmap(   )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  costmap_->getMutex(   ) ) );
           double resolution = costmap_->getResolution(   );
          
           costmap_raw_ = std::make_unique<nav2_msgs::msg::Costmap>(   );
          
           costmap_raw_->header.frame_id = global_frame_;
           costmap_raw_->header.stamp = clock_->now(   );
          
           costmap_raw_->metadata.layer = "master";
           costmap_raw_->metadata.resolution = resolution;
          
           costmap_raw_->metadata.size_x = costmap_->getSizeInCellsX(   );
           costmap_raw_->metadata.size_y = costmap_->getSizeInCellsY(   );
          
           double wx,   wy;
           costmap_->mapToWorld(  0,   0,   wx,   wy );
           costmap_raw_->metadata.origin.position.x = wx - resolution / 2;
           costmap_raw_->metadata.origin.position.y = wy - resolution / 2;
           costmap_raw_->metadata.origin.position.z = 0.0;
           costmap_raw_->metadata.origin.orientation.w = 1.0;
          
           costmap_raw_->data.resize(  costmap_raw_->metadata.size_x * costmap_raw_->metadata.size_y );
          
           unsigned char * data = costmap_->getCharMap(   );
           for (  unsigned int i = 0; i < costmap_raw_->data.size(   ); i++ ) {
           costmap_raw_->data[i] = data[i];
           }
          }
          
     184  void Costmap2DPublisher::publishCostmap(   )
          {
           if (  costmap_raw_pub_->get_subscription_count(   ) > 0 ) {
           prepareCostmap(   );
           costmap_raw_pub_->publish(  std::move(  costmap_raw_ ) );
           }
           float resolution = costmap_->getResolution(   );
          
           if (  always_send_full_costmap_ || grid_resolution != resolution ||
           grid_width != costmap_->getSizeInCellsX(   ) ||
           grid_height != costmap_->getSizeInCellsY(   ) ||
           saved_origin_x_ != costmap_->getOriginX(   ) ||
           saved_origin_y_ != costmap_->getOriginY(   ) )
           {
           if (  costmap_pub_->get_subscription_count(   ) > 0 ) {
           prepareGrid(   );
           costmap_pub_->publish(  std::move(  grid_ ) );
           }
           } else if (  x0_ < xn_ ) {
           if (  costmap_update_pub_->get_subscription_count(   ) > 0 ) {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  costmap_->getMutex(   ) ) );
           // Publish Just an Update
           auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>(   );
           update->header.stamp = rclcpp::Time(   );
           update->header.frame_id = global_frame_;
           update->x = x0_;
           update->y = y0_;
           update->width = xn_ - x0_;
           update->height = yn_ - y0_;
           update->data.resize(  update->width * update->height );
           unsigned int i = 0;
           for (  unsigned int y = y0_; y < yn_; y++ ) {
           for (  unsigned int x = x0_; x < xn_; x++ ) {
           unsigned char cost = costmap_->getCost(  x,   y );
           update->data[i++] = cost_translation_table_[cost];
           }
           }
           costmap_update_pub_->publish(  std::move(  update ) );
           }
           }
          
           xn_ = yn_ = 0;
           x0_ = costmap_->getSizeInCellsX(   );
           y0_ = costmap_->getSizeInCellsY(   );
          }
          
          void
     231  Costmap2DPublisher::costmap_service_callback(  
     232   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     233   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,  
     234   const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response )
          {
           RCLCPP_DEBUG(  logger_,   "Received costmap service request" );
          
           // TODO(  bpwilcox ): Grab correct orientation information
           tf2::Quaternion quaternion;
           quaternion.setRPY(  0.0,   0.0,   0.0 );
          
           auto size_x = costmap_->getSizeInCellsX(   );
           auto size_y = costmap_->getSizeInCellsY(   );
           auto data_length = size_x * size_y;
           unsigned char * data = costmap_->getCharMap(   );
           auto current_time = clock_->now(   );
          
           response->map.header.stamp = current_time;
           response->map.header.frame_id = global_frame_;
           response->map.metadata.size_x = size_x;
           response->map.metadata.size_y = size_y;
           response->map.metadata.resolution = costmap_->getResolution(   );
           response->map.metadata.layer = "master";
           response->map.metadata.map_load_time = current_time;
           response->map.metadata.update_time = current_time;
           response->map.metadata.origin.position.x = costmap_->getOriginX(   );
           response->map.metadata.origin.position.y = costmap_->getOriginY(   );
           response->map.metadata.origin.position.z = 0.0;
           response->map.metadata.origin.orientation = tf2::toMsg(  quaternion );
           response->map.data.resize(  data_length );
           response->map.data.assign(  data,   data + data_length );
          }
          
          } // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          #include <memory>
          #include <chrono>
          #include <string>
          #include <vector>
          #include <utility>
          
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_util/execution_timer.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "tf2_ros/create_timer_ros.h"
          #include "nav2_util/robot_utils.hpp"
          #include "rcl_interfaces/msg/set_parameters_result.hpp"
          
          using namespace std::chrono_literals;
          using std::placeholders::_1;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_costmap_2d
          {
      61  Costmap2DROS::Costmap2DROS(  const std::string & name )
          : Costmap2DROS(  name,   "/",   name ) {}
          
      64  Costmap2DROS::Costmap2DROS(  
      65   const std::string & name,  
      66   const std::string & parent_namespace,  
      67   const std::string & local_namespace )
          : nav2_util::LifecycleNode(  name,   "",  
           // NodeOption arguments take precedence over the ones provided on the command line
           // use this to make sure the node is placed on the provided namespace
           // TODO(  orduno ) Pass a sub-node instead of creating a new node for better handling
           // of the namespaces
           rclcpp::NodeOptions(   ).arguments(  {
           "--ros-args",   "-r",   std::string(  "__ns:=" ) +
           nav2_util::add_namespaces(  parent_namespace,   local_namespace ),  
           "--ros-args",   "-r",   name + ":" + std::string(  "__node:=" ) + name
           } ) ),  
           name_(  name ),  
           parent_namespace_(  parent_namespace ),  
           default_plugins_{"static_layer",   "obstacle_layer",   "inflation_layer"},  
      81   default_types_{
           "nav2_costmap_2d::StaticLayer",  
           "nav2_costmap_2d::ObstacleLayer",  
           "nav2_costmap_2d::InflationLayer"}
          {
      86   RCLCPP_INFO(  get_logger(   ),   "Creating Costmap" );
          
           std::vector<std::string> clearable_layers{"obstacle_layer",   "voxel_layer",   "range_layer"};
          
           declare_parameter(  "always_send_full_costmap",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "footprint_padding",   rclcpp::ParameterValue(  0.01f ) );
           declare_parameter(  "footprint",   rclcpp::ParameterValue(  std::string(  "[]" ) ) );
           declare_parameter(  "global_frame",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
           declare_parameter(  "height",   rclcpp::ParameterValue(  5 ) );
           declare_parameter(  "width",   rclcpp::ParameterValue(  5 ) );
           declare_parameter(  "lethal_cost_threshold",   rclcpp::ParameterValue(  100 ) );
           declare_parameter(  
           "map_topic",   rclcpp::ParameterValue(  
           (  parent_namespace_ == "/" ? "/" : parent_namespace_ + "/" ) + std::string(  "map" ) ) );
           declare_parameter(  "observation_sources",   rclcpp::ParameterValue(  std::string(  "" ) ) );
           declare_parameter(  "origin_x",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter(  "origin_y",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter(  "plugins",   rclcpp::ParameterValue(  default_plugins_ ) );
           declare_parameter(  "filters",   rclcpp::ParameterValue(  std::vector<std::string>(   ) ) );
           declare_parameter(  "publish_frequency",   rclcpp::ParameterValue(  1.0 ) );
           declare_parameter(  "resolution",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter(  "robot_base_frame",   rclcpp::ParameterValue(  std::string(  "base_link" ) ) );
           declare_parameter(  "robot_radius",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter(  "rolling_window",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "track_unknown_space",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.3 ) );
           declare_parameter(  "trinary_costmap",   rclcpp::ParameterValue(  true ) );
           declare_parameter(  "unknown_cost_value",   rclcpp::ParameterValue(  static_cast<unsigned char>(  0xff ) ) );
           declare_parameter(  "update_frequency",   rclcpp::ParameterValue(  5.0 ) );
           declare_parameter(  "use_maximum",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "clearable_layers",   rclcpp::ParameterValue(  clearable_layers ) );
          }
          
          Costmap2DROS::~Costmap2DROS(   )
          {
          }
          
          nav2_util::CallbackReturn
          Costmap2DROS::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
           getParameters(   );
          
           callback_group_ = create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,   false );
          
           // Create the costmap itself
           layered_costmap_ = std::make_unique<LayeredCostmap>(  
           global_frame_,   rolling_window_,   track_unknown_space_ );
          
           if (  !layered_costmap_->isSizeLocked(   ) ) {
           layered_costmap_->resizeMap(  
           (  unsigned int )(  map_width_meters_ / resolution_ ),  
           (  unsigned int )(  map_height_meters_ / resolution_ ),   resolution_,   origin_x_,   origin_y_ );
           }
          
           // Create the transform-related objects
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),  
           get_node_timers_interface(   ),  
           callback_group_ );
           tf_buffer_->setCreateTimerInterface(  timer_interface );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
          
           // Then load and add the plug-ins to the costmap
           for (  unsigned int i = 0; i < plugin_names_.size(   ); ++i ) {
           RCLCPP_INFO(  get_logger(   ),   "Using plugin \"%s\"",   plugin_names_[i].c_str(   ) );
          
           std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(  plugin_types_[i] );
           layered_costmap_->addPlugin(  plugin );
          
           // TODO(  mjeronimo ): instead of get(   ),   use a shared ptr
           plugin->initialize(  
           layered_costmap_.get(   ),   plugin_names_[i],   tf_buffer_.get(   ),  
           shared_from_this(   ),   callback_group_ );
          
           RCLCPP_INFO(  get_logger(   ),   "Initialized plugin \"%s\"",   plugin_names_[i].c_str(   ) );
           }
           // and costmap filters as well
           for (  unsigned int i = 0; i < filter_names_.size(   ); ++i ) {
           RCLCPP_INFO(  get_logger(   ),   "Using costmap filter \"%s\"",   filter_names_[i].c_str(   ) );
          
           std::shared_ptr<Layer> filter = plugin_loader_.createSharedInstance(  filter_types_[i] );
           layered_costmap_->addFilter(  filter );
          
           filter->initialize(  
           layered_costmap_.get(   ),   filter_names_[i],   tf_buffer_.get(   ),  
           shared_from_this(   ),   callback_group_ );
          
           RCLCPP_INFO(  get_logger(   ),   "Initialized costmap filter \"%s\"",   filter_names_[i].c_str(   ) );
           }
          
           // Create the publishers and subscribers
           footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(  
           "footprint",  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &Costmap2DROS::setRobotFootprintPolygon,   this,   std::placeholders::_1 ) );
          
           footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(  
           "published_footprint",   rclcpp::SystemDefaultsQoS(   ) );
          
           costmap_publisher_ = std::make_unique<Costmap2DPublisher>(  
           shared_from_this(   ),  
           layered_costmap_->getCostmap(   ),   global_frame_,  
           "costmap",   always_send_full_costmap_ );
          
           // Set the footprint
           if (  use_radius_ ) {
           setRobotFootprint(  makeFootprintFromRadius(  robot_radius_ ) );
           } else {
           std::vector<geometry_msgs::msg::Point> new_footprint;
           makeFootprintFromString(  footprint_,   new_footprint );
           setRobotFootprint(  new_footprint );
           }
          
           // Add cleaning service
           clear_costmap_service_ = std::make_unique<ClearCostmapService>(  shared_from_this(   ),   *this );
          
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           executor_->add_callback_group(  callback_group_,   get_node_base_interface(   ) );
           executor_thread_ = std::make_unique<nav2_util::NodeThread>(  executor_ );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          Costmap2DROS::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           costmap_publisher_->on_activate(   );
           footprint_pub_->on_activate(   );
          
           // First,   make sure that the transform between the robot base frame
           // and the global frame is available
          
           std::string tf_error;
          
           RCLCPP_INFO(  get_logger(   ),   "Checking transform" );
           rclcpp::Rate r(  2 );
           while (  rclcpp::ok(   ) &&
           !tf_buffer_->canTransform(  
           global_frame_,   robot_base_frame_,   tf2::TimePointZero,   &tf_error ) )
           {
           RCLCPP_INFO(  
           get_logger(   ),   "Timed out waiting for transform from %s to %s"
           " to become available,   tf error: %s",  
           robot_base_frame_.c_str(   ),   global_frame_.c_str(   ),   tf_error.c_str(   ) );
          
           // The error string will accumulate and errors will typically be the same,   so the last
           // will do for the warning above. Reset the string here to avoid accumulation
           tf_error.clear(   );
           r.sleep(   );
           }
          
           // Create a thread to handle updating the map
           stopped_ = true; // to active plugins
           stop_updates_ = false;
           map_update_thread_shutdown_ = false;
           map_update_thread_ = std::make_unique<std::thread>(  
           std::bind(  &Costmap2DROS::mapUpdateLoop,   this,   map_update_frequency_ ) );
          
           start(   );
          
           // Add callback for dynamic parameters
           dyn_params_handler = this->add_on_set_parameters_callback(  
           std::bind(  &Costmap2DROS::dynamicParametersCallback,   this,   _1 ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          Costmap2DROS::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           dyn_params_handler.reset(   );
           costmap_publisher_->on_deactivate(   );
           footprint_pub_->on_deactivate(   );
          
           stop(   );
          
           // Map thread stuff
           map_update_thread_shutdown_ = true;
           map_update_thread_->join(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          Costmap2DROS::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           layered_costmap_.reset(   );
          
           tf_listener_.reset(   );
           tf_buffer_.reset(   );
          
           footprint_sub_.reset(   );
           footprint_pub_.reset(   );
          
           costmap_publisher_.reset(   );
           clear_costmap_service_.reset(   );
          
           executor_thread_.reset(   );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          Costmap2DROS::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          void
          Costmap2DROS::getParameters(   )
          {
           RCLCPP_DEBUG(  get_logger(   ),   " getParameters" );
          
           // Get all of the required parameters
           get_parameter(  "always_send_full_costmap",   always_send_full_costmap_ );
           get_parameter(  "footprint",   footprint_ );
           get_parameter(  "footprint_padding",   footprint_padding_ );
           get_parameter(  "global_frame",   global_frame_ );
           get_parameter(  "height",   map_height_meters_ );
           get_parameter(  "origin_x",   origin_x_ );
           get_parameter(  "origin_y",   origin_y_ );
           get_parameter(  "publish_frequency",   map_publish_frequency_ );
           get_parameter(  "resolution",   resolution_ );
           get_parameter(  "robot_base_frame",   robot_base_frame_ );
           get_parameter(  "robot_radius",   robot_radius_ );
           get_parameter(  "rolling_window",   rolling_window_ );
           get_parameter(  "track_unknown_space",   track_unknown_space_ );
           get_parameter(  "transform_tolerance",   transform_tolerance_ );
           get_parameter(  "update_frequency",   map_update_frequency_ );
           get_parameter(  "width",   map_width_meters_ );
           get_parameter(  "plugins",   plugin_names_ );
           get_parameter(  "filters",   filter_names_ );
          
           auto node = shared_from_this(   );
          
           if (  plugin_names_ == default_plugins_ ) {
           for (  size_t i = 0; i < default_plugins_.size(   ); ++i ) {
           nav2_util::declare_parameter_if_not_declared(  
           node,   default_plugins_[i] + ".plugin",   rclcpp::ParameterValue(  default_types_[i] ) );
           }
           }
           plugin_types_.resize(  plugin_names_.size(   ) );
           filter_types_.resize(  filter_names_.size(   ) );
          
           // 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type
           for (  size_t i = 0; i < plugin_names_.size(   ); ++i ) {
           plugin_types_[i] = nav2_util::get_plugin_type_param(  node,   plugin_names_[i] );
           }
           for (  size_t i = 0; i < filter_names_.size(   ); ++i ) {
           filter_types_[i] = nav2_util::get_plugin_type_param(  node,   filter_names_[i] );
           }
          
           // 2. The map publish frequency cannot be 0 (  to avoid a divde-by-zero )
           if (  map_publish_frequency_ > 0 ) {
           publish_cycle_ = rclcpp::Duration::from_seconds(  1 / map_publish_frequency_ );
           } else {
           publish_cycle_ = rclcpp::Duration(  -1s );
           }
          
           // 3. If the footprint has been specified,   it must be in the correct format
           use_radius_ = true;
          
           if (  footprint_ != "" && footprint_ != "[]" ) {
           // Footprint parameter has been specified,   try to convert it
           std::vector<geometry_msgs::msg::Point> new_footprint;
           if (  makeFootprintFromString(  footprint_,   new_footprint ) ) {
           // The specified footprint is valid,   so we'll use that instead of the radius
           use_radius_ = false;
           } else {
           // Footprint provided but invalid,   so stay with the radius
           RCLCPP_ERROR(  
           get_logger(   ),   "The footprint parameter is invalid: \"%s\",   using radius (  %lf ) instead",  
           footprint_.c_str(   ),   robot_radius_ );
           }
           }
          }
          
          void
          Costmap2DROS::setRobotFootprint(  const std::vector<geometry_msgs::msg::Point> & points )
          {
           unpadded_footprint_ = points;
           padded_footprint_ = points;
           padFootprint(  padded_footprint_,   footprint_padding_ );
           layered_costmap_->setFootprint(  padded_footprint_ );
          }
          
          void
          Costmap2DROS::setRobotFootprintPolygon(  
           const geometry_msgs::msg::Polygon::SharedPtr footprint )
          {
           setRobotFootprint(  toPointVector(  footprint ) );
          }
          
          void
          Costmap2DROS::getOrientedFootprint(  std::vector<geometry_msgs::msg::Point> & oriented_footprint )
          {
           geometry_msgs::msg::PoseStamped global_pose;
           if (  !getRobotPose(  global_pose ) ) {
           return;
           }
          
           double yaw = tf2::getYaw(  global_pose.pose.orientation );
           transformFootprint(  
           global_pose.pose.position.x,   global_pose.pose.position.y,   yaw,  
           padded_footprint_,   oriented_footprint );
          }
          
          void
          Costmap2DROS::mapUpdateLoop(  double frequency )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "mapUpdateLoop frequency: %lf",   frequency );
          
           // the user might not want to run the loop every cycle
           if (  frequency == 0.0 ) {
           return;
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "Entering loop" );
          
           rclcpp::WallRate r(  frequency ); // 200ms by default
          
           while (  rclcpp::ok(   ) && !map_update_thread_shutdown_ ) {
           nav2_util::ExecutionTimer timer;
          
           // Measure the execution time of the updateMap method
           timer.start(   );
           updateMap(   );
           timer.end(   );
          
           RCLCPP_DEBUG(  get_logger(   ),   "Map update time: %.9f",   timer.elapsed_time_in_seconds(   ) );
           if (  publish_cycle_ > rclcpp::Duration(  0s ) && layered_costmap_->isInitialized(   ) ) {
           unsigned int x0,   y0,   xn,   yn;
           layered_costmap_->getBounds(  &x0,   &xn,   &y0,   &yn );
           costmap_publisher_->updateBounds(  x0,   xn,   y0,   yn );
          
           auto current_time = now(   );
           if (  (  last_publish_ + publish_cycle_ < current_time ) || // publish_cycle_ is due
           (  current_time < last_publish_ ) ) // time has moved backwards,   probably due to a switch to sim_time // NOLINT
           {
           RCLCPP_DEBUG(  get_logger(   ),   "Publish costmap at %s",   name_.c_str(   ) );
           costmap_publisher_->publishCostmap(   );
           last_publish_ = current_time;
           }
           }
          
           // Make sure to sleep for the remainder of our cycle time
           r.sleep(   );
          
          #if 0
           // TODO(  bpwilcox ): find ROS2 equivalent or port for r.cycletime(   )
           if (  r.period(   ) > tf2::durationFromSec(  1 / frequency ) ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Costmap2DROS: Map update loop missed its desired rate of %.4fHz... "
           "the loop actually took %.4f seconds",   frequency,   r.period(   ) );
           }
          #endif
           }
          }
          
          void
          Costmap2DROS::updateMap(   )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "Updating map..." );
          
           if (  !stop_updates_ ) {
           // get global pose
           geometry_msgs::msg::PoseStamped pose;
           if (  getRobotPose(  pose ) ) {
           const double & x = pose.pose.position.x;
           const double & y = pose.pose.position.y;
           const double yaw = tf2::getYaw(  pose.pose.orientation );
           layered_costmap_->updateMap(  x,   y,   yaw );
          
           auto footprint = std::make_unique<geometry_msgs::msg::PolygonStamped>(   );
           footprint->header = pose.header;
           transformFootprint(  x,   y,   yaw,   padded_footprint_,   *footprint );
          
           RCLCPP_DEBUG(  get_logger(   ),   "Publishing footprint" );
           footprint_pub_->publish(  std::move(  footprint ) );
           initialized_ = true;
           }
           }
          }
          
          void
          Costmap2DROS::start(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "start" );
           std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins(   );
           std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters(   );
          
           // check if we're stopped or just paused
           if (  stopped_ ) {
           // if we're stopped we need to re-subscribe to topics
           for (  std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin(   );
           plugin != plugins->end(   );
           ++plugin )
           {
           (  *plugin )->activate(   );
           }
           for (  std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin(   );
           filter != filters->end(   );
           ++filter )
           {
           (  *filter )->activate(   );
           }
           stopped_ = false;
           }
           stop_updates_ = false;
          
           // block until the costmap is re-initialized.. meaning one update cycle has run
           rclcpp::Rate r(  20.0 );
           while (  rclcpp::ok(   ) && !initialized_ ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Sleeping,   waiting for initialized_" );
           r.sleep(   );
           }
          }
          
          void
          Costmap2DROS::stop(   )
          {
           stop_updates_ = true;
           std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins(   );
           std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters(   );
           // unsubscribe from topics
           for (  std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin(   );
           plugin != plugins->end(   ); ++plugin )
           {
           (  *plugin )->deactivate(   );
           }
           for (  std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin(   );
           filter != filters->end(   ); ++filter )
           {
           (  *filter )->deactivate(   );
           }
           initialized_ = false;
           stopped_ = true;
          }
          
          void
          Costmap2DROS::pause(   )
          {
           stop_updates_ = true;
           initialized_ = false;
          }
          
          void
          Costmap2DROS::resume(   )
          {
           stop_updates_ = false;
          
           // block until the costmap is re-initialized.. meaning one update cycle has run
           rclcpp::Rate r(  100.0 );
           while (  !initialized_ ) {
           r.sleep(   );
           }
          }
          
          void
          Costmap2DROS::resetLayers(   )
          {
           Costmap2D * top = layered_costmap_->getCostmap(   );
           top->resetMap(  0,   0,   top->getSizeInCellsX(   ),   top->getSizeInCellsY(   ) );
          
           // Reset each of the plugins
           std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins(   );
           std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters(   );
           for (  std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin(   );
           plugin != plugins->end(   ); ++plugin )
           {
           (  *plugin )->reset(   );
           }
           for (  std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin(   );
           filter != filters->end(   ); ++filter )
           {
           (  *filter )->reset(   );
           }
          }
          
          bool
          Costmap2DROS::getRobotPose(  geometry_msgs::msg::PoseStamped & global_pose )
          {
           return nav2_util::getCurrentPose(  
           global_pose,   *tf_buffer_,  
           global_frame_,   robot_base_frame_,   transform_tolerance_ );
          }
          
          bool
          Costmap2DROS::transformPoseToGlobalFrame(  
           const geometry_msgs::msg::PoseStamped & input_pose,  
           geometry_msgs::msg::PoseStamped & transformed_pose )
          {
           if (  input_pose.header.frame_id == global_frame_ ) {
           transformed_pose = input_pose;
           return true;
           } else {
           return nav2_util::transformPoseInTargetFrame(  
           input_pose,   transformed_pose,   *tf_buffer_,  
           global_frame_,   transform_tolerance_ );
           }
          }
          
          rcl_interfaces::msg::SetParametersResult
          Costmap2DROS::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           auto result = rcl_interfaces::msg::SetParametersResult(   );
           bool resize_map = false;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == "robot_radius" ) {
           robot_radius_ = parameter.as_double(   );
           // Set the footprint
           if (  use_radius_ ) {
           setRobotFootprint(  makeFootprintFromRadius(  robot_radius_ ) );
           }
           } else if (  name == "footprint_padding" ) {
           footprint_padding_ = parameter.as_double(   );
           padded_footprint_ = unpadded_footprint_;
           padFootprint(  padded_footprint_,   footprint_padding_ );
           layered_costmap_->setFootprint(  padded_footprint_ );
           } else if (  name == "transform_tolerance" ) {
           transform_tolerance_ = parameter.as_double(   );
           } else if (  name == "publish_frequency" ) {
           map_publish_frequency_ = parameter.as_double(   );
           if (  map_publish_frequency_ > 0 ) {
           publish_cycle_ = rclcpp::Duration::from_seconds(  1 / map_publish_frequency_ );
           } else {
           publish_cycle_ = rclcpp::Duration(  -1s );
           }
           } else if (  name == "resolution" ) {
           resize_map = true;
           resolution_ = parameter.as_double(   );
           } else if (  name == "origin_x" ) {
           resize_map = true;
           origin_x_ = parameter.as_double(   );
           } else if (  name == "origin_y" ) {
           resize_map = true;
           origin_y_ = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == "width" ) {
           resize_map = true;
           map_width_meters_ = parameter.as_int(   );
           } else if (  name == "height" ) {
           resize_map = true;
           map_height_meters_ = parameter.as_int(   );
           }
           } else if (  type == ParameterType::PARAMETER_STRING ) {
           if (  name == "footprint" ) {
           footprint_ = parameter.as_string(   );
           std::vector<geometry_msgs::msg::Point> new_footprint;
           if (  makeFootprintFromString(  footprint_,   new_footprint ) ) {
           setRobotFootprint(  new_footprint );
           }
           } else if (  name == "robot_base_frame" ) {
           // First,   make sure that the transform between the robot base frame
           // and the global frame is available
           std::string tf_error;
           RCLCPP_INFO(  get_logger(   ),   "Checking transform" );
           if (  !tf_buffer_->canTransform(  
           global_frame_,   parameter.as_string(   ),   tf2::TimePointZero,  
           tf2::durationFromSec(  1.0 ),   &tf_error ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),   "Timed out waiting for transform from %s to %s"
           " to become available,   tf error: %s",  
           parameter.as_string(   ).c_str(   ),   global_frame_.c_str(   ),   tf_error.c_str(   ) );
           RCLCPP_WARN(  
           get_logger(   ),   "Rejecting robot_base_frame change to %s ,   leaving it to its original"
           " value of %s",   parameter.as_string(   ).c_str(   ),   robot_base_frame_.c_str(   ) );
           result.successful = false;
           return result;
           }
           robot_base_frame_ = parameter.as_string(   );
           }
           }
           }
          
           if (  resize_map && !layered_costmap_->isSizeLocked(   ) ) {
           layered_costmap_->resizeMap(  
           (  unsigned int )(  map_width_meters_ / resolution_ ),  
           (  unsigned int )(  map_height_meters_ / resolution_ ),   resolution_,   origin_x_,   origin_y_ );
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_layer.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          
          #include <nav2_costmap_2d/costmap_layer.hpp>
          #include <stdexcept>
          #include <algorithm>
          
          namespace nav2_costmap_2d
          {
          
      46  void CostmapLayer::touch(  
           double x,   double y,   double * min_x,   double * min_y,   double * max_x,  
           double * max_y )
          {
           *min_x = std::min(  x,   *min_x );
           *min_y = std::min(  y,   *min_y );
           *max_x = std::max(  x,   *max_x );
           *max_y = std::max(  y,   *max_y );
          }
          
      56  void CostmapLayer::matchSize(   )
          {
           Costmap2D * master = layered_costmap_->getCostmap(   );
           resizeMap(  
           master->getSizeInCellsX(   ),   master->getSizeInCellsY(   ),   master->getResolution(   ),  
           master->getOriginX(   ),   master->getOriginY(   ) );
          }
          
      64  void CostmapLayer::clearArea(  int start_x,   int start_y,   int end_x,   int end_y,   bool invert )
          {
           current_ = false;
           unsigned char * grid = getCharMap(   );
           for (  int x = 0; x < static_cast<int>(  getSizeInCellsX(   ) ); x++ ) {
           bool xrange = x > start_x && x < end_x;
          
           for (  int y = 0; y < static_cast<int>(  getSizeInCellsY(   ) ); y++ ) {
           if (  (  xrange && y > start_y && y < end_y ) == invert ) {
           continue;
           }
           int index = getIndex(  x,   y );
           if (  grid[index] != NO_INFORMATION ) {
           grid[index] = NO_INFORMATION;
           }
           }
           }
          }
          
      83  void CostmapLayer::addExtraBounds(  double mx0,   double my0,   double mx1,   double my1 )
          {
           extra_min_x_ = std::min(  mx0,   extra_min_x_ );
           extra_max_x_ = std::max(  mx1,   extra_max_x_ );
           extra_min_y_ = std::min(  my0,   extra_min_y_ );
           extra_max_y_ = std::max(  my1,   extra_max_y_ );
           has_extra_bounds_ = true;
          }
          
      92  void CostmapLayer::useExtraBounds(  double * min_x,   double * min_y,   double * max_x,   double * max_y )
          {
           if (  !has_extra_bounds_ ) {
           return;
           }
          
           *min_x = std::min(  extra_min_x_,   *min_x );
           *min_y = std::min(  extra_min_y_,   *min_y );
           *max_x = std::max(  extra_max_x_,   *max_x );
           *max_y = std::max(  extra_max_y_,   *max_y );
           extra_min_x_ = 1e6;
           extra_min_y_ = 1e6;
           extra_max_x_ = -1e6;
           extra_max_y_ = -1e6;
           has_extra_bounds_ = false;
          }
          
     109  void CostmapLayer::updateWithMax(  
     110   nav2_costmap_2d::Costmap2D & master_grid,   int min_i,   int min_j,  
           int max_i,  
           int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
          
           unsigned char * master_array = master_grid.getCharMap(   );
           unsigned int span = master_grid.getSizeInCellsX(   );
          
           for (  int j = min_j; j < max_j; j++ ) {
           unsigned int it = j * span + min_i;
           for (  int i = min_i; i < max_i; i++ ) {
           if (  costmap_[it] == NO_INFORMATION ) {
           it++;
           continue;
           }
          
           unsigned char old_cost = master_array[it];
           if (  old_cost == NO_INFORMATION || old_cost < costmap_[it] ) {
           master_array[it] = costmap_[it];
           }
           it++;
           }
           }
          }
          
     138  void CostmapLayer::updateWithTrueOverwrite(  
     139   nav2_costmap_2d::Costmap2D & master_grid,   int min_i,  
           int min_j,  
           int max_i,  
           int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
          
           if (  costmap_ == nullptr ) {
           throw std::runtime_error(  "Can't update costmap layer: It has't been initialized yet!" );
           }
          
           unsigned char * master = master_grid.getCharMap(   );
           unsigned int span = master_grid.getSizeInCellsX(   );
          
           for (  int j = min_j; j < max_j; j++ ) {
           unsigned int it = span * j + min_i;
           for (  int i = min_i; i < max_i; i++ ) {
           master[it] = costmap_[it];
           it++;
           }
           }
          }
          
     164  void CostmapLayer::updateWithOverwrite(  
     165   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
           unsigned char * master = master_grid.getCharMap(   );
           unsigned int span = master_grid.getSizeInCellsX(   );
          
           for (  int j = min_j; j < max_j; j++ ) {
           unsigned int it = span * j + min_i;
           for (  int i = min_i; i < max_i; i++ ) {
           if (  costmap_[it] != NO_INFORMATION ) {
           master[it] = costmap_[it];
           }
           it++;
           }
           }
          }
          
     185  void CostmapLayer::updateWithAddition(  
     186   nav2_costmap_2d::Costmap2D & master_grid,  
           int min_i,   int min_j,   int max_i,   int max_j )
          {
           if (  !enabled_ ) {
           return;
           }
           unsigned char * master_array = master_grid.getCharMap(   );
           unsigned int span = master_grid.getSizeInCellsX(   );
          
           for (  int j = min_j; j < max_j; j++ ) {
           unsigned int it = j * span + min_i;
           for (  int i = min_i; i < max_i; i++ ) {
           if (  costmap_[it] == NO_INFORMATION ) {
           it++;
           continue;
           }
          
           unsigned char old_cost = master_array[it];
           if (  old_cost == NO_INFORMATION ) {
           master_array[it] = costmap_[it];
           } else {
           int sum = old_cost + costmap_[it];
           if (  sum >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ) {
           master_array[it] = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
           } else {
           master_array[it] = sum;
           }
           }
           it++;
           }
           }
          }
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_math.cpp

       1  /*
           * Copyright (  c ) 2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <nav2_costmap_2d/costmap_math.hpp>
          
          #include <vector>
          
      34  double distanceToLine(  double pX,   double pY,   double x0,   double y0,   double x1,   double y1 )
          {
           double A = pX - x0;
           double B = pY - y0;
           double C = x1 - x0;
           double D = y1 - y0;
          
           double dot = A * C + B * D;
           double len_sq = C * C + D * D;
           double param = dot / len_sq;
          
           double xx,   yy;
          
           if (  param < 0 ) {
           xx = x0;
           yy = y0;
           } else if (  param > 1 ) {
           xx = x1;
           yy = y1;
           } else {
           xx = x0 + param * C;
           yy = y0 + param * D;
           }
          
           return distance(  pX,   pY,   xx,   yy );
          }

navigation2/nav2_costmap_2d/src/costmap_subscriber.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          
          namespace nav2_costmap_2d
          {
          
      23  CostmapSubscriber::CostmapSubscriber(  
      24   const nav2_util::LifecycleNode::WeakPtr & parent,  
      25   const std::string & topic_name )
          : topic_name_(  topic_name )
          {
           auto node = parent.lock(   );
           costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(  
           topic_name_,  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &CostmapSubscriber::costmapCallback,   this,   std::placeholders::_1 ) );
          }
          
      35  CostmapSubscriber::CostmapSubscriber(  
      36   const rclcpp::Node::WeakPtr & parent,  
      37   const std::string & topic_name )
          : topic_name_(  topic_name )
          {
           auto node = parent.lock(   );
           costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(  
           topic_name_,  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &CostmapSubscriber::costmapCallback,   this,   std::placeholders::_1 ) );
          }
          
      47  std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap(   )
          {
           if (  !costmap_received_ ) {
           throw std::runtime_error(  "Costmap is not available" );
           }
           toCostmap2D(   );
           return costmap_;
          }
          
      56  void CostmapSubscriber::toCostmap2D(   )
          {
           auto current_costmap_msg = std::atomic_load(  &costmap_msg_ );
          
           if (  costmap_ == nullptr ) {
           costmap_ = std::make_shared<Costmap2D>(  
           current_costmap_msg->metadata.size_x,   current_costmap_msg->metadata.size_y,  
           current_costmap_msg->metadata.resolution,   current_costmap_msg->metadata.origin.position.x,  
           current_costmap_msg->metadata.origin.position.y );
           } else if (  costmap_->getSizeInCellsX(   ) != current_costmap_msg->metadata.size_x || // NOLINT
           costmap_->getSizeInCellsY(   ) != current_costmap_msg->metadata.size_y ||
           costmap_->getResolution(   ) != current_costmap_msg->metadata.resolution ||
           costmap_->getOriginX(   ) != current_costmap_msg->metadata.origin.position.x ||
           costmap_->getOriginY(   ) != current_costmap_msg->metadata.origin.position.y )
           {
           // Update the size of the costmap
           costmap_->resizeMap(  
           current_costmap_msg->metadata.size_x,   current_costmap_msg->metadata.size_y,  
           current_costmap_msg->metadata.resolution,  
           current_costmap_msg->metadata.origin.position.x,  
           current_costmap_msg->metadata.origin.position.y );
           }
          
           unsigned char * master_array = costmap_->getCharMap(   );
           unsigned int index = 0;
           for (  unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i ) {
           for (  unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j ) {
           master_array[index] = current_costmap_msg->data[index];
           ++index;
           }
           }
          }
          
      89  void CostmapSubscriber::costmapCallback(  const nav2_msgs::msg::Costmap::SharedPtr msg )
          {
           std::atomic_store(  &costmap_msg_,   msg );
           if (  !costmap_received_ ) {
           costmap_received_ = true;
           }
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Modified by: Shivang Patel (  shivaan14@gmail.com )
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <algorithm>
          #include <iostream>
          
          #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
          
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_costmap_2d/exceptions.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          #include "nav2_util/line_iterator.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_costmap_2d
          {
          
      35  CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(  
      36   CostmapSubscriber & costmap_sub,  
      37   FootprintSubscriber & footprint_sub,  
      38   std::string name )
          : name_(  name ),  
           costmap_sub_(  costmap_sub ),  
           footprint_sub_(  footprint_sub ),  
           collision_checker_(  nullptr )
          {}
          
      45  bool CostmapTopicCollisionChecker::isCollisionFree(  
      46   const geometry_msgs::msg::Pose2D & pose,  
      47   bool fetch_costmap_and_footprint )
          {
           try {
           if (  scorePose(  pose,   fetch_costmap_and_footprint ) >= LETHAL_OBSTACLE ) {
           return false;
           }
           return true;
           } catch (  const IllegalPoseException & e ) {
           RCLCPP_ERROR(  rclcpp::get_logger(  name_ ),   "%s",   e.what(   ) );
           return false;
           } catch (  const CollisionCheckerException & e ) {
           RCLCPP_ERROR(  rclcpp::get_logger(  name_ ),   "%s",   e.what(   ) );
           return false;
           } catch (  ... ) {
           RCLCPP_ERROR(  rclcpp::get_logger(  name_ ),   "Failed to check pose score!" );
           return false;
           }
          }
          
      66  double CostmapTopicCollisionChecker::scorePose(  
      67   const geometry_msgs::msg::Pose2D & pose,  
      68   bool fetch_costmap_and_footprint )
          {
           if (  fetch_costmap_and_footprint ) {
           try {
           collision_checker_.setCostmap(  costmap_sub_.getCostmap(   ) );
           } catch (  const std::runtime_error & e ) {
           throw CollisionCheckerException(  e.what(   ) );
           }
           }
          
           unsigned int cell_x,   cell_y;
           if (  !collision_checker_.worldToMap(  pose.x,   pose.y,   cell_x,   cell_y ) ) {
           RCLCPP_DEBUG(  rclcpp::get_logger(  name_ ),   "Map Cell: [%d,   %d]",   cell_x,   cell_y );
           throw IllegalPoseException(  name_,   "Pose Goes Off Grid." );
           }
          
           return collision_checker_.footprintCost(  getFootprint(  pose,   fetch_costmap_and_footprint ) );
          }
          
      87  Footprint CostmapTopicCollisionChecker::getFootprint(  
      88   const geometry_msgs::msg::Pose2D & pose,  
      89   bool fetch_latest_footprint )
          {
           if (  fetch_latest_footprint ) {
           std_msgs::msg::Header header;
           if (  !footprint_sub_.getFootprintInRobotFrame(  footprint_,   header ) ) {
           throw CollisionCheckerException(  "Current footprint not available." );
           }
           }
           Footprint footprint;
           transformFootprint(  pose.x,   pose.y,   pose.theta,   footprint_,   footprint );
          
           return footprint;
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint.cpp

       1  /*
           * Copyright (  c ) 2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #include "nav2_costmap_2d/footprint.hpp"
          
          #include <algorithm>
          #include <limits>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/point32.hpp"
          #include "nav2_costmap_2d/array_parser.hpp"
          #include "nav2_costmap_2d/costmap_math.hpp"
          
          namespace nav2_costmap_2d
          {
          
      43  void calculateMinAndMaxDistances(  
           const std::vector<geometry_msgs::msg::Point> & footprint,  
           double & min_dist,   double & max_dist )
          {
           min_dist = std::numeric_limits<double>::max(   );
           max_dist = 0.0;
          
           if (  footprint.size(   ) <= 2 ) {
           return;
           }
          
           for (  unsigned int i = 0; i < footprint.size(   ) - 1; ++i ) {
           // check the distance from the robot center point to the first vertex
           double vertex_dist = distance(  0.0,   0.0,   footprint[i].x,   footprint[i].y );
           double edge_dist = distanceToLine(  
           0.0,   0.0,   footprint[i].x,   footprint[i].y,  
           footprint[i + 1].x,   footprint[i + 1].y );
           min_dist = std::min(  min_dist,   std::min(  vertex_dist,   edge_dist ) );
           max_dist = std::max(  max_dist,   std::max(  vertex_dist,   edge_dist ) );
           }
          
           // we also need to do the last vertex and the first vertex
           double vertex_dist = distance(  0.0,   0.0,   footprint.back(   ).x,   footprint.back(   ).y );
           double edge_dist = distanceToLine(  
           0.0,   0.0,   footprint.back(   ).x,   footprint.back(   ).y,  
           footprint.front(   ).x,   footprint.front(   ).y );
           min_dist = std::min(  min_dist,   std::min(  vertex_dist,   edge_dist ) );
           max_dist = std::max(  max_dist,   std::max(  vertex_dist,   edge_dist ) );
          }
          
      73  geometry_msgs::msg::Point32 toPoint32(  geometry_msgs::msg::Point pt )
          {
           geometry_msgs::msg::Point32 point32;
           point32.x = pt.x;
           point32.y = pt.y;
           point32.z = pt.z;
           return point32;
          }
          
      82  geometry_msgs::msg::Point toPoint(  geometry_msgs::msg::Point32 pt )
          {
           geometry_msgs::msg::Point point;
           point.x = pt.x;
           point.y = pt.y;
           point.z = pt.z;
           return point;
          }
          
      91  geometry_msgs::msg::Polygon toPolygon(  std::vector<geometry_msgs::msg::Point> pts )
          {
           geometry_msgs::msg::Polygon polygon;
           for (  unsigned int i = 0; i < pts.size(   ); i++ ) {
           polygon.points.push_back(  toPoint32(  pts[i] ) );
           }
           return polygon;
          }
          
     100  std::vector<geometry_msgs::msg::Point> toPointVector(  geometry_msgs::msg::Polygon::SharedPtr polygon )
          {
           std::vector<geometry_msgs::msg::Point> pts;
           for (  unsigned int i = 0; i < polygon->points.size(   ); i++ ) {
           pts.push_back(  toPoint(  polygon->points[i] ) );
           }
           return pts;
          }
          
     109  void transformFootprint(  
           double x,   double y,   double theta,  
           const std::vector<geometry_msgs::msg::Point> & footprint_spec,  
           std::vector<geometry_msgs::msg::Point> & oriented_footprint )
          {
           // build the oriented footprint at a given location
           oriented_footprint.resize(  footprint_spec.size(   ) );
           double cos_th = cos(  theta );
           double sin_th = sin(  theta );
           for (  unsigned int i = 0; i < footprint_spec.size(   ); ++i ) {
           double new_x = x + (  footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th );
           double new_y = y + (  footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th );
           geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
           new_pt.x = new_x;
           new_pt.y = new_y;
           }
          }
          
     127  void transformFootprint(  
           double x,   double y,   double theta,  
           const std::vector<geometry_msgs::msg::Point> & footprint_spec,  
           geometry_msgs::msg::PolygonStamped & oriented_footprint )
          {
           // build the oriented footprint at a given location
           oriented_footprint.polygon.points.clear(   );
           double cos_th = cos(  theta );
           double sin_th = sin(  theta );
           for (  unsigned int i = 0; i < footprint_spec.size(   ); ++i ) {
           geometry_msgs::msg::Point32 new_pt;
           new_pt.x = x + (  footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th );
           new_pt.y = y + (  footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th );
           oriented_footprint.polygon.points.push_back(  new_pt );
           }
          }
          
     144  void padFootprint(  std::vector<geometry_msgs::msg::Point> & footprint,   double padding )
          {
           // pad footprint in place
           for (  unsigned int i = 0; i < footprint.size(   ); i++ ) {
           geometry_msgs::msg::Point & pt = footprint[i];
           pt.x += sign0(  pt.x ) * padding;
           pt.y += sign0(  pt.y ) * padding;
           }
          }
          
          
     155  std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(  double radius )
          {
           std::vector<geometry_msgs::msg::Point> points;
          
           // Loop over 16 angles around a circle making a point each time
           int N = 16;
           geometry_msgs::msg::Point pt;
           for (  int i = 0; i < N; ++i ) {
           double angle = i * 2 * M_PI / N;
           pt.x = cos(  angle ) * radius;
           pt.y = sin(  angle ) * radius;
          
           points.push_back(  pt );
           }
          
           return points;
          }
          
          
     174  bool makeFootprintFromString(  
           const std::string & footprint_string,  
           std::vector<geometry_msgs::msg::Point> & footprint )
          {
           std::string error;
           std::vector<std::vector<float>> vvf = parseVVF(  footprint_string,   error );
          
           if (  error != "" ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),   "Error parsing footprint parameter: '%s'",   error.c_str(   ) );
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),   " Footprint string was '%s'.",   footprint_string.c_str(   ) );
           return false;
           }
          
           // convert vvf into points.
           if (  vvf.size(   ) < 3 ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),  
           "You must specify at least three points for the robot footprint,   reverting to previous footprint." ); //NOLINT
           return false;
           }
           footprint.reserve(  vvf.size(   ) );
           for (  unsigned int i = 0; i < vvf.size(   ); i++ ) {
           if (  vvf[i].size(   ) == 2 ) {
           geometry_msgs::msg::Point point;
           point.x = vvf[i][0];
           point.y = vvf[i][1];
           point.z = 0;
           footprint.push_back(  point );
           } else {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),  
           "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.",   //NOLINT
           static_cast<int>(  vvf[i].size(   ) ) );
           return false;
           }
           }
          
           return true;
          }
          
          } // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint_collision_checker.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Modified by: Shivang Patel (  shivaang14@gmail.com )
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <algorithm>
          
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_costmap_2d/exceptions.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          #include "nav2_util/line_iterator.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_costmap_2d
          {
          
          template<typename CostmapT>
      35  FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(   )
          : costmap_(  nullptr )
          {
          }
          
          template<typename CostmapT>
      41  FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(  
      42   CostmapT costmap )
          : costmap_(  costmap )
          {
          }
          
          template<typename CostmapT>
      48  double FootprintCollisionChecker<CostmapT>::footprintCost(  const Footprint footprint )
          {
           // now we really have to lay down the footprint in the costmap_ grid
           unsigned int x0,   x1,   y0,   y1;
           double footprint_cost = 0.0;
          
           // get the cell coord of the first point
           if (  !worldToMap(  footprint[0].x,   footprint[0].y,   x0,   y0 ) ) {
           return static_cast<double>(  LETHAL_OBSTACLE );
           }
          
           // cache the start to eliminate a worldToMap call
           unsigned int xstart = x0;
           unsigned int ystart = y0;
          
           // we need to rasterize each line in the footprint
           for (  unsigned int i = 0; i < footprint.size(   ) - 1; ++i ) {
           // get the cell coord of the second point
           if (  !worldToMap(  footprint[i + 1].x,   footprint[i + 1].y,   x1,   y1 ) ) {
           return static_cast<double>(  LETHAL_OBSTACLE );
           }
          
           footprint_cost = std::max(  lineCost(  x0,   x1,   y0,   y1 ),   footprint_cost );
          
           // the second point is next iteration's first point
           x0 = x1;
           y0 = y1;
          
           // if in collision,   no need to continue
           if (  footprint_cost == static_cast<double>(  LETHAL_OBSTACLE ) ) {
           return footprint_cost;
           }
           }
          
           // we also need to connect the first point in the footprint to the last point
           // the last iteration's x1,   y1 are the last footprint point's coordinates
           return std::max(  lineCost(  xstart,   x1,   ystart,   y1 ),   footprint_cost );
          }
          
          template<typename CostmapT>
      88  double FootprintCollisionChecker<CostmapT>::lineCost(  int x0,   int x1,   int y0,   int y1 ) const
          {
           double line_cost = 0.0;
           double point_cost = -1.0;
          
           for (  nav2_util::LineIterator line(  x0,   y0,   x1,   y1 ); line.isValid(   ); line.advance(   ) ) {
           point_cost = pointCost(  line.getX(   ),   line.getY(   ) ); // Score the current point
          
           // if in collision,   no need to continue
           if (  point_cost == static_cast<double>(  LETHAL_OBSTACLE ) ) {
           return point_cost;
           }
          
           if (  line_cost < point_cost ) {
           line_cost = point_cost;
           }
           }
          
           return line_cost;
          }
          
          template<typename CostmapT>
     110  bool FootprintCollisionChecker<CostmapT>::worldToMap(  
           double wx,   double wy,   unsigned int & mx,   unsigned int & my )
          {
           return costmap_->worldToMap(  wx,   wy,   mx,   my );
          }
          
          template<typename CostmapT>
     117  double FootprintCollisionChecker<CostmapT>::pointCost(  int x,   int y ) const
          {
           return costmap_->getCost(  x,   y );
          }
          
          template<typename CostmapT>
     123  void FootprintCollisionChecker<CostmapT>::setCostmap(  CostmapT costmap )
          {
           costmap_ = costmap;
          }
          
          template<typename CostmapT>
     129  double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(  
     130   double x,   double y,   double theta,   const Footprint footprint )
          {
           double cos_th = cos(  theta );
           double sin_th = sin(  theta );
           Footprint oriented_footprint;
           for (  unsigned int i = 0; i < footprint.size(   ); ++i ) {
           geometry_msgs::msg::Point new_pt;
           new_pt.x = x + (  footprint[i].x * cos_th - footprint[i].y * sin_th );
           new_pt.y = y + (  footprint[i].x * sin_th + footprint[i].y * cos_th );
           oriented_footprint.push_back(  new_pt );
           }
          
           return footprintCost(  oriented_footprint );
          }
          
          // declare our valid template parameters
     146  template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
          template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <vector>
          #include <memory>
          
          #include "nav2_costmap_2d/footprint_subscriber.hpp"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          namespace nav2_costmap_2d
          {
          
      28  FootprintSubscriber::FootprintSubscriber(  
      29   const nav2_util::LifecycleNode::WeakPtr & parent,  
      30   const std::string & topic_name,  
      31   tf2_ros::Buffer & tf,  
      32   std::string robot_base_frame,  
           double transform_tolerance )
          : tf_(  tf ),  
           robot_base_frame_(  robot_base_frame ),  
           transform_tolerance_(  transform_tolerance )
          {
           auto node = parent.lock(   );
           footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(  
           topic_name,   rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &FootprintSubscriber::footprint_callback,   this,   std::placeholders::_1 ) );
          }
          
      44  FootprintSubscriber::FootprintSubscriber(  
      45   const rclcpp::Node::WeakPtr & parent,  
      46   const std::string & topic_name,  
      47   tf2_ros::Buffer & tf,  
      48   std::string robot_base_frame,  
           double transform_tolerance )
          : tf_(  tf ),  
           robot_base_frame_(  robot_base_frame ),  
           transform_tolerance_(  transform_tolerance )
          {
           auto node = parent.lock(   );
           footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(  
           topic_name,   rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &FootprintSubscriber::footprint_callback,   this,   std::placeholders::_1 ) );
          }
          
          bool
      61  FootprintSubscriber::getFootprintRaw(  
      62   std::vector<geometry_msgs::msg::Point> & footprint,  
      63   std_msgs::msg::Header & footprint_header )
          {
           if (  !footprint_received_ ) {
           return false;
           }
          
           auto current_footprint = std::atomic_load(  &footprint_ );
           footprint = toPointVector(  
           std::make_shared<geometry_msgs::msg::Polygon>(  current_footprint->polygon ) );
           footprint_header = current_footprint->header;
          
           return true;
          }
          
          bool
      78  FootprintSubscriber::getFootprintInRobotFrame(  
      79   std::vector<geometry_msgs::msg::Point> & footprint,  
      80   std_msgs::msg::Header & footprint_header )
          {
           if (  !getFootprintRaw(  footprint,   footprint_header ) ) {
           return false;
           }
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  
           current_pose,   tf_,   footprint_header.frame_id,   robot_base_frame_,  
           transform_tolerance_,   footprint_header.stamp ) )
           {
           return false;
           }
          
           double x = current_pose.pose.position.x;
           double y = current_pose.pose.position.y;
           double theta = tf2::getYaw(  current_pose.pose.orientation );
          
           std::vector<geometry_msgs::msg::Point> temp;
           transformFootprint(  -x,   -y,   0,   footprint,   temp );
           transformFootprint(  0,   0,   -theta,   temp,   footprint );
          
           footprint_header.frame_id = robot_base_frame_;
           footprint_header.stamp = current_pose.header.stamp;
          
           return true;
          }
          
          void
     109  FootprintSubscriber::footprint_callback(  const geometry_msgs::msg::PolygonStamped::SharedPtr msg )
          {
           std::atomic_store(  &footprint_,   msg );
           if (  !footprint_received_ ) {
           footprint_received_ = true;
           }
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/layer.cpp

       1  /*
           * Copyright (  c ) 2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav2_costmap_2d/layer.hpp"
          
          #include <string>
          #include <vector>
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_costmap_2d
          {
          
      39  Layer::Layer(   )
          : layered_costmap_(  nullptr ),  
           name_(   ),  
           tf_(  nullptr ),  
           current_(  false ),  
           enabled_(  false )
          {}
          
          void
      48  Layer::initialize(  
      49   LayeredCostmap * parent,  
      50   std::string name,  
      51   tf2_ros::Buffer * tf,  
      52   const nav2_util::LifecycleNode::WeakPtr & node,  
      53   rclcpp::CallbackGroup::SharedPtr callback_group )
          {
           layered_costmap_ = parent;
           name_ = name;
           tf_ = tf;
           node_ = node;
           callback_group_ = callback_group;
           {
           auto node_shared_ptr = node_.lock(   );
           logger_ = node_shared_ptr->get_logger(   );
           clock_ = node_shared_ptr->get_clock(   );
           }
          
           onInitialize(   );
          }
          
          const std::vector<geometry_msgs::msg::Point> &
      70  Layer::getFootprint(   ) const
          {
           return layered_costmap_->getFootprint(   );
          }
          
          void
      76  Layer::declareParameter(  
      77   const std::string & param_name,  
      78   const rclcpp::ParameterValue & value )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           local_params_.insert(  param_name );
           nav2_util::declare_parameter_if_not_declared(  
           node,   getFullName(  param_name ),   value );
          }
          
          void
      90  Layer::declareParameter(  
      91   const std::string & param_name,  
      92   const rclcpp::ParameterType & param_type )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           local_params_.insert(  param_name );
           nav2_util::declare_parameter_if_not_declared(  
           node,   getFullName(  param_name ),   param_type );
          }
          
          bool
     104  Layer::hasParameter(  const std::string & param_name )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           return node->has_parameter(  getFullName(  param_name ) );
          }
          
          std::string
     114  Layer::getFullName(  const std::string & param_name )
          {
           return std::string(  name_ + "." + param_name );
          }
          
          } // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/layered_costmap.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           * David V. Lu!!
           *********************************************************************/
          #include "nav2_costmap_2d/layered_costmap.hpp"
          
          #include <algorithm>
          #include <cstdio>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "nav2_costmap_2d/footprint.hpp"
          
          
          using std::vector;
          
          namespace nav2_costmap_2d
          {
          
      55  LayeredCostmap::LayeredCostmap(  std::string global_frame,   bool rolling_window,   bool track_unknown )
          : primary_costmap_(   ),   combined_costmap_(   ),  
           global_frame_(  global_frame ),  
           rolling_window_(  rolling_window ),  
           current_(  false ),  
           minx_(  0.0 ),  
           miny_(  0.0 ),  
           maxx_(  0.0 ),  
           maxy_(  0.0 ),  
           bx0_(  0 ),  
           bxn_(  0 ),  
           by0_(  0 ),  
           byn_(  0 ),  
           initialized_(  false ),  
           size_locked_(  false ),  
           circumscribed_radius_(  1.0 ),  
           inscribed_radius_(  0.1 )
          {
           if (  track_unknown ) {
           primary_costmap_.setDefaultValue(  255 );
           combined_costmap_.setDefaultValue(  255 );
           } else {
           primary_costmap_.setDefaultValue(  0 );
           combined_costmap_.setDefaultValue(  0 );
           }
          }
          
      82  LayeredCostmap::~LayeredCostmap(   )
          {
           while (  plugins_.size(   ) > 0 ) {
           plugins_.pop_back(   );
           }
           while (  filters_.size(   ) > 0 ) {
           filters_.pop_back(   );
           }
          }
          
      92  void LayeredCostmap::addPlugin(  std::shared_ptr<Layer> plugin )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  combined_costmap_.getMutex(   ) ) );
           plugins_.push_back(  plugin );
          }
          
      98  void LayeredCostmap::resizeMap(  
           unsigned int size_x,   unsigned int size_y,   double resolution,  
           double origin_x,  
           double origin_y,  
     102   bool size_locked )
          {
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  combined_costmap_.getMutex(   ) ) );
           size_locked_ = size_locked;
           primary_costmap_.resizeMap(  size_x,   size_y,   resolution,   origin_x,   origin_y );
           combined_costmap_.resizeMap(  size_x,   size_y,   resolution,   origin_x,   origin_y );
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   ); ++plugin )
           {
           (  *plugin )->matchSize(   );
           }
           for (  vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin(   );
           filter != filters_.end(   ); ++filter )
           {
           (  *filter )->matchSize(   );
           }
          }
          
     120  bool LayeredCostmap::isOutofBounds(  double robot_x,   double robot_y )
          {
           unsigned int mx,   my;
           return !combined_costmap_.worldToMap(  robot_x,   robot_y,   mx,   my );
          }
          
     126  void LayeredCostmap::updateMap(  double robot_x,   double robot_y,   double robot_yaw )
          {
           // Lock for the remainder of this function,   some plugins (  e.g. VoxelLayer )
           // implement thread unsafe updateBounds(   ) functions.
           std::unique_lock<Costmap2D::mutex_t> lock(  *(  combined_costmap_.getMutex(   ) ) );
          
           // if we're using a rolling buffer costmap...
           // we need to update the origin using the robot's position
           if (  rolling_window_ ) {
           double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX(   ) / 2;
           double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY(   ) / 2;
           primary_costmap_.updateOrigin(  new_origin_x,   new_origin_y );
           combined_costmap_.updateOrigin(  new_origin_x,   new_origin_y );
           }
          
           if (  isOutofBounds(  robot_x,   robot_y ) ) {
           RCLCPP_WARN(  
           rclcpp::get_logger(  "nav2_costmap_2d" ),  
           "Robot is out of bounds of the costmap!" );
           }
          
           if (  plugins_.size(   ) == 0 && filters_.size(   ) == 0 ) {
           return;
           }
          
           minx_ = miny_ = std::numeric_limits<double>::max(   );
           maxx_ = maxy_ = std::numeric_limits<double>::lowest(   );
          
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   ); ++plugin )
           {
           double prev_minx = minx_;
           double prev_miny = miny_;
           double prev_maxx = maxx_;
           double prev_maxy = maxy_;
           (  *plugin )->updateBounds(  robot_x,   robot_y,   robot_yaw,   &minx_,   &miny_,   &maxx_,   &maxy_ );
           if (  minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy ) {
           RCLCPP_WARN(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),   "Illegal bounds change,   was [tl: (  %f,   %f ),   br: (  %f,   %f )],   but "
           "is now [tl: (  %f,   %f ),   br: (  %f,   %f )]. The offending layer is %s",  
           prev_minx,   prev_miny,   prev_maxx,   prev_maxy,  
           minx_,   miny_,   maxx_,   maxy_,  
           (  *plugin )->getName(   ).c_str(   ) );
           }
           }
           for (  vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin(   );
           filter != filters_.end(   ); ++filter )
           {
           double prev_minx = minx_;
           double prev_miny = miny_;
           double prev_maxx = maxx_;
           double prev_maxy = maxy_;
           (  *filter )->updateBounds(  robot_x,   robot_y,   robot_yaw,   &minx_,   &miny_,   &maxx_,   &maxy_ );
           if (  minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy ) {
           RCLCPP_WARN(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),   "Illegal bounds change,   was [tl: (  %f,   %f ),   br: (  %f,   %f )],   but "
           "is now [tl: (  %f,   %f ),   br: (  %f,   %f )]. The offending filter is %s",  
           prev_minx,   prev_miny,   prev_maxx,   prev_maxy,  
           minx_,   miny_,   maxx_,   maxy_,  
           (  *filter )->getName(   ).c_str(   ) );
           }
           }
          
           int x0,   xn,   y0,   yn;
           combined_costmap_.worldToMapEnforceBounds(  minx_,   miny_,   x0,   y0 );
           combined_costmap_.worldToMapEnforceBounds(  maxx_,   maxy_,   xn,   yn );
          
           x0 = std::max(  0,   x0 );
           xn = std::min(  static_cast<int>(  combined_costmap_.getSizeInCellsX(   ) ),   xn + 1 );
           y0 = std::max(  0,   y0 );
           yn = std::min(  static_cast<int>(  combined_costmap_.getSizeInCellsY(   ) ),   yn + 1 );
          
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  
           "nav2_costmap_2d" ),   "Updating area x: [%d,   %d] y: [%d,   %d]",   x0,   xn,   y0,   yn );
          
           if (  xn < x0 || yn < y0 ) {
           return;
           }
          
           if (  filters_.size(   ) == 0 ) {
           // If there are no filters enabled just update costmap sequentially by each plugin
           combined_costmap_.resetMap(  x0,   y0,   xn,   yn );
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   ); ++plugin )
           {
           (  *plugin )->updateCosts(  combined_costmap_,   x0,   y0,   xn,   yn );
           }
           } else {
           // Costmap Filters enabled
           // 1. Update costmap by plugins
           primary_costmap_.resetMap(  x0,   y0,   xn,   yn );
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   ); ++plugin )
           {
           (  *plugin )->updateCosts(  primary_costmap_,   x0,   y0,   xn,   yn );
           }
          
           // 2. Copy processed costmap window to a final costmap.
           // primary_costmap_ remain to be untouched for further usage by plugins.
           if (  !combined_costmap_.copyWindow(  primary_costmap_,   x0,   y0,   xn,   yn,   x0,   y0 ) ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "nav2_costmap_2d" ),  
           "Can not copy costmap (  %i,  %i )..(  %i,  %i ) window",  
           x0,   y0,   xn,   yn );
           throw std::runtime_error{"Can not copy costmap"};
           }
          
           // 3. Apply filters over the plugins in order to make filters' work
           // not being considered by plugins on next updateMap(   ) calls
           for (  vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin(   );
           filter != filters_.end(   ); ++filter )
           {
           (  *filter )->updateCosts(  combined_costmap_,   x0,   y0,   xn,   yn );
           }
           }
          
           bx0_ = x0;
           bxn_ = xn;
           by0_ = y0;
           byn_ = yn;
          
           initialized_ = true;
          }
          
     253  bool LayeredCostmap::isCurrent(   )
          {
           current_ = true;
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   ); ++plugin )
           {
           current_ = current_ && (  *plugin )->isCurrent(   );
           }
           for (  vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin(   );
           filter != filters_.end(   ); ++filter )
           {
           current_ = current_ && (  *filter )->isCurrent(   );
           }
           return current_;
          }
          
     269  void LayeredCostmap::setFootprint(  const std::vector<geometry_msgs::msg::Point> & footprint_spec )
          {
           footprint_ = footprint_spec;
           nav2_costmap_2d::calculateMinAndMaxDistances(  
           footprint_spec,  
           inscribed_radius_,   circumscribed_radius_ );
          
           for (  vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(   );
           plugin != plugins_.end(   );
           ++plugin )
           {
           (  *plugin )->onFootprintChanged(   );
           }
           for (  vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin(   );
           filter != filters_.end(   );
           ++filter )
           {
           (  *filter )->onFootprintChanged(   );
           }
          }
          
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/observation_buffer.cpp

       1  /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           *********************************************************************/
          #include "nav2_costmap_2d/observation_buffer.hpp"
          
          #include <algorithm>
          #include <list>
          #include <string>
          #include <vector>
          #include <chrono>
          
          #include "tf2/convert.h"
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          using namespace std::chrono_literals;
          
          namespace nav2_costmap_2d
          {
      51  ObservationBuffer::ObservationBuffer(  
      52   const nav2_util::LifecycleNode::WeakPtr & parent,  
      53   std::string topic_name,  
           double observation_keep_time,  
           double expected_update_rate,  
           double min_obstacle_height,   double max_obstacle_height,   double obstacle_max_range,  
           double obstacle_min_range,  
      58   double raytrace_max_range,   double raytrace_min_range,   tf2_ros::Buffer & tf2_buffer,  
      59   std::string global_frame,  
      60   std::string sensor_frame,  
      61   tf2::Duration tf_tolerance )
          : tf2_buffer_(  tf2_buffer ),  
           observation_keep_time_(  rclcpp::Duration::from_seconds(  observation_keep_time ) ),  
           expected_update_rate_(  rclcpp::Duration::from_seconds(  expected_update_rate ) ),  
           global_frame_(  global_frame ),  
           sensor_frame_(  sensor_frame ),  
           topic_name_(  topic_name ),  
           min_obstacle_height_(  min_obstacle_height ),   max_obstacle_height_(  max_obstacle_height ),  
           obstacle_max_range_(  obstacle_max_range ),   obstacle_min_range_(  obstacle_min_range ),  
           raytrace_max_range_(  raytrace_max_range ),   raytrace_min_range_(  
           raytrace_min_range ),   tf_tolerance_(  tf_tolerance )
          {
           auto node = parent.lock(   );
           clock_ = node->get_clock(   );
           logger_ = node->get_logger(   );
           last_updated_ = node->now(   );
          }
          
      79  ObservationBuffer::~ObservationBuffer(   )
          {
          }
          
      83  void ObservationBuffer::bufferCloud(  const sensor_msgs::msg::PointCloud2 & cloud )
          {
           geometry_msgs::msg::PointStamped global_origin;
          
           // create a new observation on the list to be populated
           observation_list_.push_front(  Observation(   ) );
          
           // check whether the origin frame has been set explicitly
           // or whether we should get it from the cloud
           std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
          
           try {
           // given these observations come from sensors...
           // we'll need to store the origin pt of the sensor
           geometry_msgs::msg::PointStamped local_origin;
           local_origin.header.stamp = cloud.header.stamp;
           local_origin.header.frame_id = origin_frame;
           local_origin.point.x = 0;
           local_origin.point.y = 0;
           local_origin.point.z = 0;
           tf2_buffer_.transform(  local_origin,   global_origin,   global_frame_,   tf_tolerance_ );
           tf2::convert(  global_origin.point,   observation_list_.front(   ).origin_ );
          
           // make sure to pass on the raytrace/obstacle range
           // of the observation buffer to the observations
           observation_list_.front(   ).raytrace_max_range_ = raytrace_max_range_;
           observation_list_.front(   ).raytrace_min_range_ = raytrace_min_range_;
           observation_list_.front(   ).obstacle_max_range_ = obstacle_max_range_;
           observation_list_.front(   ).obstacle_min_range_ = obstacle_min_range_;
          
           sensor_msgs::msg::PointCloud2 global_frame_cloud;
          
           // transform the point cloud
           tf2_buffer_.transform(  cloud,   global_frame_cloud,   global_frame_,   tf_tolerance_ );
           global_frame_cloud.header.stamp = cloud.header.stamp;
          
           // now we need to remove observations from the cloud that are below
           // or above our height thresholds
           sensor_msgs::msg::PointCloud2 & observation_cloud = *(  observation_list_.front(   ).cloud_ );
           observation_cloud.height = global_frame_cloud.height;
           observation_cloud.width = global_frame_cloud.width;
           observation_cloud.fields = global_frame_cloud.fields;
           observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
           observation_cloud.point_step = global_frame_cloud.point_step;
           observation_cloud.row_step = global_frame_cloud.row_step;
           observation_cloud.is_dense = global_frame_cloud.is_dense;
          
           unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width;
           sensor_msgs::PointCloud2Modifier modifier(  observation_cloud );
           modifier.resize(  cloud_size );
           unsigned int point_count = 0;
          
           // copy over the points that are within our height bounds
           sensor_msgs::PointCloud2Iterator<float> iter_z(  global_frame_cloud,   "z" );
           std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(   ),  
           iter_global_end = global_frame_cloud.data.end(   );
           std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin(   );
           for (  ; iter_global != iter_global_end; ++iter_z,   iter_global +=
           global_frame_cloud.point_step )
           {
           if (  (  *iter_z ) <= max_obstacle_height_ &&
           (  *iter_z ) >= min_obstacle_height_ )
           {
           std::copy(  iter_global,   iter_global + global_frame_cloud.point_step,   iter_obs );
           iter_obs += global_frame_cloud.point_step;
           ++point_count;
           }
           }
          
           // resize the cloud for the number of legal points
           modifier.resize(  point_count );
           observation_cloud.header.stamp = cloud.header.stamp;
           observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
           } catch (  tf2::TransformException & ex ) {
           // if an exception occurs,   we need to remove the empty observation from the list
           observation_list_.pop_front(   );
           RCLCPP_ERROR(  
           logger_,  
           "TF Exception that should never happen for sensor frame: %s,   cloud frame: %s,   %s",  
           sensor_frame_.c_str(   ),  
           cloud.header.frame_id.c_str(   ),   ex.what(   ) );
           return;
           }
          
           // if the update was successful,   we want to update the last updated time
           last_updated_ = clock_->now(   );
          
           // we'll also remove any stale observations from the list
           purgeStaleObservations(   );
          }
          
          // returns a copy of the observations
     175  void ObservationBuffer::getObservations(  std::vector<Observation> & observations )
          {
           // first... let's make sure that we don't have any stale observations
           purgeStaleObservations(   );
          
           // now we'll just copy the observations for the caller
           std::list<Observation>::iterator obs_it;
           for (  obs_it = observation_list_.begin(   ); obs_it != observation_list_.end(   ); ++obs_it ) {
           observations.push_back(  *obs_it );
           }
          }
          
     187  void ObservationBuffer::purgeStaleObservations(   )
          {
           if (  !observation_list_.empty(   ) ) {
           std::list<Observation>::iterator obs_it = observation_list_.begin(   );
           // if we're keeping observations for no time... then we'll only keep one observation
           if (  observation_keep_time_ == rclcpp::Duration(  0.0s ) ) {
           observation_list_.erase(  ++obs_it,   observation_list_.end(   ) );
           return;
           }
          
           // otherwise... we'll have to loop through the observations to see which ones are stale
           for (  obs_it = observation_list_.begin(   ); obs_it != observation_list_.end(   ); ++obs_it ) {
           Observation & obs = *obs_it;
           // check if the observation is out of date... and if it is,  
           // remove it and those that follow from the list
           if (  (  clock_->now(   ) - obs.cloud_->header.stamp ) >
           observation_keep_time_ )
           {
           observation_list_.erase(  obs_it,   observation_list_.end(   ) );
           return;
           }
           }
           }
          }
          
     212  bool ObservationBuffer::isCurrent(   ) const
          {
           if (  expected_update_rate_ == rclcpp::Duration(  0.0s ) ) {
           return true;
           }
          
           bool current = (  clock_->now(   ) - last_updated_ ) <=
           expected_update_rate_;
           if (  !current ) {
           RCLCPP_WARN(  
           logger_,  
           "The %s observation buffer has not been updated for %.2f seconds,   "
           "and it should be updated every %.2f seconds.",  
           topic_name_.c_str(   ),  
           (  clock_->now(   ) - last_updated_ ).seconds(   ),  
           expected_update_rate_.seconds(   ) );
           }
           return current;
          }
          
     232  void ObservationBuffer::resetLastUpdated(   )
          {
           last_updated_ = clock_->now(   );
          }
          } // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/test/integration/costmap_tester.cpp

       1  /*********************************************************************
          *
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2009,   Willow Garage,   Inc.
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of Willow Garage,   Inc. nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Eitan Marder-Eppstein
          *********************************************************************/
          #include <memory>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
          namespace nav2_costmap_2d
          {
          
          std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
          
      50  class CostmapTester : public testing::Test
          {
          public:
      53   explicit CostmapTester(  tf2_ros::Buffer & tf );
      54   void checkConsistentCosts(   );
      55   void compareCellToNeighbors(  
      56   nav2_costmap_2d::Costmap2D & costmap,  
           unsigned int x,   unsigned int y );
      58   void compareCells(  
      59   nav2_costmap_2d::Costmap2D & costmap,  
           unsigned int x,   unsigned int y,   unsigned int nx,   unsigned int ny );
      61   virtual void TestBody(   ) {}
          };
          
      64  CostmapTester::CostmapTester(  tf2_ros::Buffer & tf )
          {
           costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap",   tf );
          }
          
      69  void CostmapTester::checkConsistentCosts(   )
          {
           nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(   );
          
           // get a copy of the costmap contained by our ros wrapper
           costmap->saveMap(  "costmap_test.pgm" );
          
           // loop through the costmap and check for any unexpected drop-offs in costs
           for (  unsigned int i = 0; i < costmap->getSizeInCellsX(   ); ++i ) {
           for (  unsigned int j = 0; j < costmap->getSizeInCellsY(   ); ++j ) {
           compareCellToNeighbors(  *costmap,   i,   j );
           }
           }
          }
          
      84  void CostmapTester::compareCellToNeighbors(  
      85   nav2_costmap_2d::Costmap2D & costmap,  
           unsigned int x,   unsigned int y )
          {
           // we'll compare the cost of this cell with that of
           // its eight neighbors to see if they're reasonable
           for (  int offset_x = -1; offset_x <= 1; ++offset_x ) {
           for (  int offset_y = -1; offset_y <= 1; ++offset_y ) {
           int nx = x + offset_x;
           int ny = y + offset_y;
          
           // check to make sure that the neighbor cell is a legal one
           if (  nx >= 0 && nx < static_cast<int>(  costmap.getSizeInCellsX(   ) ) && ny >= 0 &&
           ny < static_cast<int>(  costmap.getSizeInCellsY(   ) ) )
           {
           compareCells(  costmap,   x,   y,   nx,   ny );
           }
           }
           }
          }
          
          // for all lethal and inscribed costs,  
          // we'll make sure that their neighbors have the cost values we'd expect
     107  void CostmapTester::compareCells(  
     108   nav2_costmap_2d::Costmap2D & costmap,  
           unsigned int x,   unsigned int y,   unsigned int nx,   unsigned int ny )
          {
           double cell_distance = hypot(  static_cast<int>(  x - nx ),   static_cast<int>(  y - ny ) );
          
           unsigned char cell_cost = costmap.getCost(  x,   y );
           unsigned char neighbor_cost = costmap.getCost(  nx,   ny );
          
           if (  cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           // if the cell is a lethal obstacle,  
           // then we know that all its neighbors should have equal or slighlty less cost
           unsigned char expected_lowest_cost = 0;
           EXPECT_TRUE(  
           neighbor_cost >= expected_lowest_cost ||
           (  cell_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE ) );
           } else if (  cell_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ) {
           // the furthest valid distance from an obstacle
           // is the inscribed radius plus the cell distance away
           double furthest_valid_distance = 0;
           unsigned char expected_lowest_cost = 0;
           if (  neighbor_cost < expected_lowest_cost ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "costmap_tester" ),  
           "Cell cost (  %d,   %d ): %d,   neighbor cost (  %d,   %d ): %d,   expected lowest cost: %d,   cell distance: %.2f,   furthest valid distance: %.2f",   // NOLINT
           x,   y,   cell_cost,   nx,   ny,   neighbor_cost,   expected_lowest_cost,  
           cell_distance,   furthest_valid_distance );
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "costmap_tester" ),   "Cell: (  %d,   %d ),   Neighbor: (  %d,   %d )",  
           x,   y,   nx,   ny );
           costmap.saveMap(  "failing_costmap.pgm" );
           }
           EXPECT_TRUE(  
           neighbor_cost >= expected_lowest_cost ||
           (  furthest_valid_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE ) );
           }
          }
          } // namespace nav2_costmap_2d
          
          nav2_costmap_2d::CostmapTester * map_tester = NULL;
          tf2_ros::TransformListener * tfl_;
          tf2_ros::Buffer * tf_;
          
     151  TEST(  CostmapTester,   checkConsistentCosts ) {
           map_tester->checkConsistentCosts(   );
          }
          
     155  void testCallback(   )
          {
           int test_result = RUN_ALL_TESTS(   );
           RCLCPP_INFO(  rclcpp::get_logger(  "costmap_tester" ),   "gtest return value: %d",   test_result );
          }
          
     161  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
           testing::InitGoogleTest(  &argc,   argv );
          
           tf_ = new tf2_ros::Buffer(  node->get_clock(   ) );
           tfl_ = new tf2_ros::TransformListener(  *tf_ );
           map_tester = new nav2_costmap_2d::CostmapTester(  *tf_ );
           rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(  30000ms,   testCallback );
           rclcpp::spin(  costmap_ros_ );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_costmap_2d/test/integration/dyn_params_tests.cpp

       1  // Copyright (  c ) 2021 Wyca Robotics
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "tf2_ros/transform_broadcaster.h"
          
      24  class RclCppFixture
          {
          public:
      27   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      28   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      32  class DynParamTestNode
          {
          public:
      35   DynParamTestNode(   ) {}
      36   ~DynParamTestNode(   ) {}
          };
          
      39  TEST(  DynParamTestNode,   testDynParamsSet )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "dyn_param_tester" );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
          
           // Set tf between default global_frame and robot_base_frame in order not to block in on_activate
           std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
           std::make_unique<tf2_ros::TransformBroadcaster>(  node );
           geometry_msgs::msg::TransformStamped t;
           t.header.stamp = node->get_clock(   )->now(   );
           t.header.frame_id = "map";
           t.child_frame_id = "base_link";
           tf_broadcaster_->sendTransform(  t );
           t.header.frame_id = "map";
           t.child_frame_id = "test_frame";
           tf_broadcaster_->sendTransform(  t );
          
           costmap->on_activate(  rclcpp_lifecycle::State(   ) );
          
           auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           node->shared_from_this(   ),  
           "/test_costmap/test_costmap",  
           rmw_qos_profile_parameters );
           auto results1 = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "robot_radius",   1.234 ),  
           rclcpp::Parameter(  "footprint_padding",   2.345 ),  
           rclcpp::Parameter(  "transform_tolerance",   3.456 ),  
           rclcpp::Parameter(  "publish_frequency",   4.567 ),  
           rclcpp::Parameter(  "resolution",   5.678 ),  
           rclcpp::Parameter(  "origin_x",   6.789 ),  
           rclcpp::Parameter(  "origin_y",   7.891 ),  
           rclcpp::Parameter(  "width",   2 ),  
           rclcpp::Parameter(  "height",   3 ),  
           rclcpp::Parameter(  
           "footprint",  
           "[[-0.325,   -0.325],   [-0.325,   0.325],   [0.325,   0.325],   [0.46,   0.0],   [0.325,   -0.325]]" ),  
           rclcpp::Parameter(  "robot_base_frame",   "test_frame" ),  
           } );
          
           // Try setting robot_base_frame to an invalid frame,   should be rejected
           auto results2 = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "robot_base_frame",   "wrong_test_frame" ),  
           } );
          
           rclcpp::spin_some(  costmap->get_node_base_interface(   ) );
          
           EXPECT_EQ(  costmap->get_parameter(  "robot_radius" ).as_double(   ),   1.234 );
           EXPECT_EQ(  costmap->get_parameter(  "footprint_padding" ).as_double(   ),   2.345 );
           EXPECT_EQ(  costmap->get_parameter(  "transform_tolerance" ).as_double(   ),   3.456 );
           EXPECT_EQ(  costmap->get_parameter(  "publish_frequency" ).as_double(   ),   4.567 );
           EXPECT_EQ(  costmap->get_parameter(  "resolution" ).as_double(   ),   5.678 );
           EXPECT_EQ(  costmap->get_parameter(  "origin_x" ).as_double(   ),   6.789 );
           EXPECT_EQ(  costmap->get_parameter(  "origin_y" ).as_double(   ),   7.891 );
           EXPECT_EQ(  costmap->get_parameter(  "width" ).as_int(   ),   2 );
           EXPECT_EQ(  costmap->get_parameter(  "height" ).as_int(   ),   3 );
           EXPECT_EQ(  
           costmap->get_parameter(  "footprint" ).as_string(   ),  
           "[[-0.325,   -0.325],   [-0.325,   0.325],   [0.325,   0.325],   [0.46,   0.0],   [0.325,   -0.325]]" );
           EXPECT_EQ(  costmap->get_parameter(  "robot_base_frame" ).as_string(   ),   "test_frame" );
          
           costmap->on_deactivate(  rclcpp_lifecycle::State(   ) );
           costmap->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap->on_shutdown(  rclcpp_lifecycle::State(   ) );
          }

navigation2/nav2_costmap_2d/test/integration/footprint_tests.cpp

       1  /*********************************************************************
          *
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2009,   Willow Garage,   Inc.
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of Willow Garage,   Inc. nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Dave Hershberger
          *********************************************************************/
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "nav2_costmap_2d/footprint.hpp"
          
      47  class RclCppFixture
          {
          public:
      50   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      51   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      55  class FootprintTestNode
          {
          public:
      58   FootprintTestNode(   )
           {
           // Default footprint padding and footprint radius from Costmap2DROS
           testFootprint(  0.01f,   0.1 );
           }
          
      64   ~FootprintTestNode(   ) {}
          
      66   void testFootprint(  double footprint_padding,   std::string footprint )
           {
           footprint_padding_ = footprint_padding;
           if (  footprint != "" && footprint != "[]" ) {
           std::vector<geometry_msgs::msg::Point> new_footprint;
           if (  nav2_costmap_2d::makeFootprintFromString(  footprint,   new_footprint ) ) {
           setRobotFootprint(  new_footprint );
           } else {
           RCLCPP_ERROR(  rclcpp::get_logger(  "footprint_tester" ),   "Invalid footprint string" );
           }
           }
           }
          
      79   void testFootprint(  double footprint_padding,   double robot_radius )
           {
           footprint_padding_ = footprint_padding;
           setRobotFootprint(  nav2_costmap_2d::makeFootprintFromRadius(  robot_radius ) );
           }
          
      85   std::vector<geometry_msgs::msg::Point> getRobotFootprint(   )
           {
           return footprint_;
           }
          
          protected:
      91   void setRobotFootprint(  const std::vector<geometry_msgs::msg::Point> & points )
           {
           footprint_ = points;
           nav2_costmap_2d::padFootprint(  footprint_,   footprint_padding_ );
           }
          
           double footprint_padding_;
      98   std::vector<geometry_msgs::msg::Point> footprint_;
          };
          
     101  class TestNode : public ::testing::Test
          {
          public:
     104   TestNode(   )
           {
           footprint_tester_ = std::make_shared<FootprintTestNode>(   );
           }
          
     109   ~TestNode(   ) {}
          
          protected:
     112   std::shared_ptr<FootprintTestNode> footprint_tester_;
          };
          
          // Start with empty test before updating test footprints
     116  TEST_F(  TestNode,   footprint_empty )
          {
           // FootprintTestNode cm(  "costmap_footprint_empty",   *tf_ );
           std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint(   );
           // With no specification of footprint or radius,  
           // defaults to 0.1 meter radius plus 0.01 meter padding.
           EXPECT_EQ(  16u,   footprint.size(   ) );
          
           EXPECT_NEAR(  0.11f,   footprint[0].x,   0.0001 );
           EXPECT_NEAR(  0.0f,   footprint[0].y,   0.0001 );
           EXPECT_EQ(  0.0f,   footprint[0].z );
          }
          
     129  TEST_F(  TestNode,   unpadded_footprint_from_string_param )
          {
           footprint_tester_->testFootprint(  0.0,   "[[1,   1],   [-1,   1],   [-1,   -1]]" );
          
           std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint(   );
           EXPECT_EQ(  3u,   footprint.size(   ) );
          
           EXPECT_EQ(  1.0f,   footprint[0].x );
           EXPECT_EQ(  1.0f,   footprint[0].y );
           EXPECT_EQ(  0.0f,   footprint[0].z );
          
           EXPECT_EQ(  -1.0f,   footprint[1].x );
           EXPECT_EQ(  1.0f,   footprint[1].y );
           EXPECT_EQ(  0.0f,   footprint[1].z );
          
           EXPECT_EQ(  -1.0f,   footprint[2].x );
           EXPECT_EQ(  -1.0f,   footprint[2].y );
           EXPECT_EQ(  0.0f,   footprint[2].z );
          }
          
     149  TEST_F(  TestNode,   padded_footprint_from_string_param )
          {
           footprint_tester_->testFootprint(  0.5,   "[[1,   1],   [-1,   1],   [-1,   -1]]" );
          
           std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint(   );
           EXPECT_EQ(  3u,   footprint.size(   ) );
          
           EXPECT_EQ(  1.5f,   footprint[0].x );
           EXPECT_EQ(  1.5f,   footprint[0].y );
           EXPECT_EQ(  0.0f,   footprint[0].z );
          
           EXPECT_EQ(  -1.5f,   footprint[1].x );
           EXPECT_EQ(  1.5f,   footprint[1].y );
           EXPECT_EQ(  0.0f,   footprint[1].z );
          
           EXPECT_EQ(  -1.5f,   footprint[2].x );
           EXPECT_EQ(  -1.5f,   footprint[2].y );
           EXPECT_EQ(  0.0f,   footprint[2].z );
          }
          
     169  TEST_F(  TestNode,   radius_param )
          {
           footprint_tester_->testFootprint(  0,   10.0 );
           std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint(   );
           // Circular robot has 16-point footprint auto-generated.
           EXPECT_EQ(  16u,   footprint.size(   ) );
          
           // Check the first point
           EXPECT_EQ(  10.0f,   footprint[0].x );
           EXPECT_EQ(  0.0f,   footprint[0].y );
           EXPECT_EQ(  0.0f,   footprint[0].z );
          
           // Check the 4th point,   which should be 90 degrees around the circle from the first.
           EXPECT_NEAR(  0.0f,   footprint[4].x,   0.0001 );
           EXPECT_NEAR(  10.0f,   footprint[4].y,   0.0001 );
           EXPECT_EQ(  0.0f,   footprint[4].z );
          }
          
     187  TEST_F(  TestNode,   footprint_from_same_level_param )
          {
           footprint_tester_->testFootprint(  0.0,   "[[1,   2],   [3,   4],   [5,   6]]" );
           std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint(   );
           EXPECT_EQ(  3u,   footprint.size(   ) );
          
           EXPECT_EQ(  1.0f,   footprint[0].x );
           EXPECT_EQ(  2.0f,   footprint[0].y );
           EXPECT_EQ(  0.0f,   footprint[0].z );
          
           EXPECT_EQ(  3.0f,   footprint[1].x );
           EXPECT_EQ(  4.0f,   footprint[1].y );
           EXPECT_EQ(  0.0f,   footprint[1].z );
          
           EXPECT_EQ(  5.0f,   footprint[2].x );
           EXPECT_EQ(  6.0f,   footprint[2].y );
           EXPECT_EQ(  0.0f,   footprint[2].z );
          }

navigation2/nav2_costmap_2d/test/integration/inflation_tests.cpp

       1  /*
           * Copyright (  c ) 2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          /**
           * @author David Lu!!
           * Test harness for InflationLayer for Costmap2D
           */
          #include <gtest/gtest.h>
          
          #include <cmath>
          #include <map>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/obstacle_layer.hpp"
          #include "nav2_costmap_2d/inflation_layer.hpp"
          #include "nav2_costmap_2d/observation_buffer.hpp"
          #include "../testing_helper.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          using geometry_msgs::msg::Point;
          using nav2_costmap_2d::CellData;
          
      54  class RclCppFixture
          {
          public:
      57   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      58   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      62  class TestNode : public ::testing::Test
          {
          public:
      65   TestNode(   ) {}
          
      67   ~TestNode(   ) {}
          
      69   std::vector<Point> setRadii(  
      70   nav2_costmap_2d::LayeredCostmap & layers,  
           double length,   double width );
          
      73   void validatePointInflation(  
           unsigned int mx,   unsigned int my,  
      75   nav2_costmap_2d::Costmap2D * costmap,  
      76   std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,  
           double inflation_radius );
          
      79   void initNode(  std::vector<rclcpp::Parameter> parameters );
      80   void initNode(  double inflation_radius );
          
      82   void waitForMap(  std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer );
          
          protected:
      85   nav2_util::LifecycleNode::SharedPtr node_;
          };
          
      88  std::vector<Point> TestNode::setRadii(  
      89   nav2_costmap_2d::LayeredCostmap & layers,  
           double length,   double width )
          {
           std::vector<Point> polygon;
           Point p;
           p.x = width;
           p.y = length;
           polygon.push_back(  p );
           p.x = width;
           p.y = -length;
           polygon.push_back(  p );
           p.x = -width;
           p.y = -length;
           polygon.push_back(  p );
           p.x = -width;
           p.y = length;
           polygon.push_back(  p );
           layers.setFootprint(  polygon );
          
           return polygon;
          }
          
     111  void TestNode::waitForMap(  std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer )
          {
           while (  !slayer->isCurrent(   ) ) {
           rclcpp::spin_some(  node_->get_node_base_interface(   ) );
           }
          }
          
          // Test that a single point gets inflated properly
     119  void TestNode::validatePointInflation(  
           unsigned int mx,   unsigned int my,  
     121   nav2_costmap_2d::Costmap2D * costmap,  
     122   std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,  
           double inflation_radius )
          {
           bool * seen = new bool[costmap->getSizeInCellsX(   ) * costmap->getSizeInCellsY(   )];
           memset(  seen,   false,   costmap->getSizeInCellsX(   ) * costmap->getSizeInCellsY(   ) * sizeof(  bool ) );
           std::map<double,   std::vector<CellData>> m;
           CellData initial(  costmap->getIndex(  mx,   my ),   mx,   my,   mx,   my );
           m[0].push_back(  initial );
           for (  std::map<double,   std::vector<CellData>>::iterator bin = m.begin(   );
           bin != m.end(   ); ++bin )
           {
           for (  unsigned int i = 0; i < bin->second.size(   ); ++i ) {
           const CellData cell = bin->second[i];
           if (  !seen[cell.index_] ) {
           seen[cell.index_] = true;
           unsigned int dx = (  cell.x_ > cell.src_x_ ) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
           unsigned int dy = (  cell.y_ > cell.src_y_ ) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
           double dist = std::hypot(  dx,   dy );
          
           unsigned char expected_cost = ilayer->computeCost(  dist );
           ASSERT_TRUE(  costmap->getCost(  cell.x_,   cell.y_ ) >= expected_cost );
          
           if (  dist > inflation_radius ) {
           continue;
           }
          
           if (  dist == bin->first ) {
           // Adding to our current bin could cause a reallocation
           // Which appears to cause the iterator to get messed up
           dist += 0.001;
           }
          
           if (  cell.x_ > 0 ) {
           CellData data(  costmap->getIndex(  cell.x_ - 1,   cell.y_ ),  
           cell.x_ - 1,   cell.y_,   cell.src_x_,   cell.src_y_ );
           m[dist].push_back(  data );
           }
           if (  cell.y_ > 0 ) {
           CellData data(  costmap->getIndex(  cell.x_,   cell.y_ - 1 ),  
           cell.x_,   cell.y_ - 1,   cell.src_x_,   cell.src_y_ );
           m[dist].push_back(  data );
           }
           if (  cell.x_ < costmap->getSizeInCellsX(   ) - 1 ) {
           CellData data(  costmap->getIndex(  cell.x_ + 1,   cell.y_ ),  
           cell.x_ + 1,   cell.y_,   cell.src_x_,   cell.src_y_ );
           m[dist].push_back(  data );
           }
           if (  cell.y_ < costmap->getSizeInCellsY(   ) - 1 ) {
           CellData data(  costmap->getIndex(  cell.x_,   cell.y_ + 1 ),  
           cell.x_,   cell.y_ + 1,   cell.src_x_,   cell.src_y_ );
           m[dist].push_back(  data );
           }
           }
           }
           }
           delete[] seen;
          }
          
     180  void TestNode::initNode(  std::vector<rclcpp::Parameter> parameters )
          {
           auto options = rclcpp::NodeOptions(   );
           options.parameter_overrides(  parameters );
          
           node_ = std::make_shared<nav2_util::LifecycleNode>(  
           "inflation_test_node",   "",   options );
          
           // Declare non-plugin specific costmap parameters
           node_->declare_parameter(  "map_topic",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
           node_->declare_parameter(  "track_unknown_space",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "use_maximum",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "lethal_cost_threshold",   rclcpp::ParameterValue(  100 ) );
           node_->declare_parameter(  
           "unknown_cost_value",  
           rclcpp::ParameterValue(  static_cast<unsigned char>(  0xff ) ) );
           node_->declare_parameter(  "trinary_costmap",   rclcpp::ParameterValue(  true ) );
           node_->declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.3 ) );
           node_->declare_parameter(  "observation_sources",   rclcpp::ParameterValue(  std::string(  "" ) ) );
          }
          
     201  void TestNode::initNode(  double inflation_radius )
          {
           std::vector<rclcpp::Parameter> parameters;
           // Set cost_scaling_factor parameter to 1.0 for inflation layer
           parameters.push_back(  rclcpp::Parameter(  "inflation.cost_scaling_factor",   1.0 ) );
           parameters.push_back(  rclcpp::Parameter(  "inflation.inflation_radius",   inflation_radius ) );
          
           initNode(  parameters );
          }
          
     211  TEST_F(  TestNode,   testAdjacentToObstacleCanStillMove )
          {
           initNode(  4.1 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   2.1,   2.3 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           addObservation(  olayer,   0,   0,   MAX_Z );
          
           layers.updateMap(  0,   0,   0 );
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           // printMap(  *costmap );
           EXPECT_EQ(  nav2_costmap_2d::LETHAL_OBSTACLE,   costmap->getCost(  0,   0 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   costmap->getCost(  1,   0 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   costmap->getCost(  2,   0 ) );
           EXPECT_TRUE(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(  3,   0 ) );
           EXPECT_TRUE(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(  2,   1 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   costmap->getCost(  1,   1 ) );
          }
          
     243  TEST_F(  TestNode,   testInflationShouldNotCreateUnknowns )
          {
           initNode(  4.1 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   2.1,   2.3 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           addObservation(  olayer,   0,   0,   MAX_Z );
          
           layers.updateMap(  0,   0,   0 );
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
          
           EXPECT_EQ(  countValues(  *costmap,   nav2_costmap_2d::NO_INFORMATION ),   0u );
          }
          
     270  TEST_F(  TestNode,   testInflationInUnkown )
          {
           std::vector<rclcpp::Parameter> parameters;
           // Set cost_scaling_factor parameter to 1.0 for inflation layer
           parameters.push_back(  rclcpp::Parameter(  "inflation.cost_scaling_factor",   1.0 ) );
           parameters.push_back(  rclcpp::Parameter(  "inflation.inflation_radius",   4.1 ) );
           parameters.push_back(  rclcpp::Parameter(  "inflation.inflate_unknown",   true ) );
          
           initNode(  parameters );
          
           node_->set_parameter(  rclcpp::Parameter(  "track_unknown_space",   true ) );
          
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   true );
           layers.resizeMap(  9,   9,   1,   0,   0 );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   2.1,   2.3 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
           layers.setFootprint(  polygon );
          
           addObservation(  olayer,   4,   4,   MAX_Z,   0.0,   0.0,   MAX_Z,   true,   false );
          
           layers.updateMap(  0,   0,   0 );
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
          
           // Only the 4 corners of the map should remain unknown
           EXPECT_EQ(  countValues(  *costmap,   nav2_costmap_2d::NO_INFORMATION ),   4u );
          }
          
     305  TEST_F(  TestNode,   testInflationAroundUnkown )
          {
           auto inflation_radius = 4.1;
           std::vector<rclcpp::Parameter> parameters;
           // Set cost_scaling_factor parameter to 1.0 for inflation layer
           parameters.push_back(  rclcpp::Parameter(  "inflation.cost_scaling_factor",   1.0 ) );
           parameters.push_back(  rclcpp::Parameter(  "inflation.inflation_radius",   inflation_radius ) );
           parameters.push_back(  rclcpp::Parameter(  "inflation.inflate_around_unknown",   true ) );
          
           initNode(  parameters );
          
           node_->set_parameter(  rclcpp::Parameter(  "track_unknown_space",   true ) );
          
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   2.1,   2.3 );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
           layers.setFootprint(  polygon );
           layers.updateMap(  0,   0,   0 );
          
           layers.getCostmap(   )->setCost(  4,   4,   nav2_costmap_2d::NO_INFORMATION );
           ilayer->updateCosts(  *layers.getCostmap(   ),   0,   0,   10,   10 );
          
           validatePointInflation(  4,   4,   layers.getCostmap(   ),   ilayer,   inflation_radius );
          }
          
          /**
           * Test for the cost function correctness with a larger range and different values
           */
     340  TEST_F(  TestNode,   testCostFunctionCorrectness )
          {
           initNode(  10.5 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           layers.resizeMap(  100,   100,   1,   0,   0 );
           // Footprint with inscribed radius = 5.0
           // circumscribed radius = 8.0
           std::vector<Point> polygon = setRadii(  layers,   5.0,   6.25 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           addObservation(  olayer,   50,   50,   MAX_Z );
          
           layers.updateMap(  0,   0,   0 );
           nav2_costmap_2d::Costmap2D * map = layers.getCostmap(   );
          
           // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
           // unsigned char c = ilayer->computeCost(  8.0 );
           // ASSERT_EQ(  ilayer->getCircumscribedCost(   ),   c );
          
           for (  unsigned int i = 0; i <= (  unsigned int )ceil(  5.0 ); i++ ) {
           // To the right
           ASSERT_EQ(  map->getCost(  50 + i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map->getCost(  50 + i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // To the left
           ASSERT_EQ(  map->getCost(  50 - i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map->getCost(  50 - i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // Down
           ASSERT_EQ(  map->getCost(  50,   50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map->getCost(  50,   50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // Up
           ASSERT_EQ(  map->getCost(  50,   50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map->getCost(  50,   50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           }
          
           // Verify the normalized cost attenuates as expected
           for (  unsigned int i = (  unsigned int )(  ceil(  5.0 ) + 1 ); i <= (  unsigned int )ceil(  10.5 ); i++ ) {
           unsigned char expectedValue = ilayer->computeCost(  i / 1.0 );
           ASSERT_EQ(  map->getCost(  50 + i,   50 ),   expectedValue );
           }
          
           // Update with no hits. Should clear (  revert to the static map
           /*map->resetMapOutsideWindow(  0,   0,   0.0,   0.0 );
           cloud.points.resize(  0 );
          
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           Observation obs2(  p,   cloud,   100.0,   100.0 );
           std::vector<Observation> obsBuf2;
           obsBuf2.push_back(  obs2 );
          
           map->updateWorld(  0,   0,   obsBuf2,   obsBuf2 );
          
           for(  unsigned int i = 0; i < 100; i++ )
           for(  unsigned int j = 0; j < 100; j++ )
           ASSERT_EQ(  map->getCost(  i,   j ),   nav2_costmap_2d::FREE_SPACE );*/
          }
          
          /**
           * Test that there is no regression and that costs do not get
           * underestimated with the distance-as-key map used to replace
           * the previously used priority queue. This is a more thorough
           * test of the cost function being correctly applied.
           */
     414  TEST_F(  TestNode,   testInflationOrderCorrectness )
          {
           const double inflation_radius = 4.1;
           initNode(  inflation_radius );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   2.1,   2.3 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           // Add two diagonal cells,   they would induce problems under the
           // previous implementations
           addObservation(  olayer,   4,   4,   MAX_Z );
           addObservation(  olayer,   5,   5,   MAX_Z );
          
           layers.updateMap(  0,   0,   0 );
          
           validatePointInflation(  4,   4,   layers.getCostmap(   ),   ilayer,   inflation_radius );
           validatePointInflation(  5,   5,   layers.getCostmap(   ),   ilayer,   inflation_radius );
          }
          
          /**
           * Test inflation for both static and dynamic obstacles
           */
     448  TEST_F(  TestNode,   testInflation )
          {
           initNode(  1 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   1,   1 );
          
           std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
           addStaticLayer(  layers,   tf,   node_,   slayer );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
           layers.setFootprint(  polygon );
          
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           waitForMap(  slayer );
          
           layers.updateMap(  0,   0,   0 );
           // printMap(  *costmap );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   20u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   28u );
          
           /*/ Iterate over all id's and verify they are obstacles
           for(  std::vector<unsigned int>::const_iterator it = occupiedCells.begin(   ); it != occupiedCells.end(   ); ++it ){
           unsigned int ind = *it;
           unsigned int x,   y;
           map.indexToCells(  ind,   x,   y );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  x,   y ) ),   true );
           ASSERT_EQ(  map.getCost(  x,   y ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  x,   y ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           }*/
          
           addObservation(  olayer,   0,   0,   0.4 );
           layers.updateMap(  0,   0,   0 );
          
           // It and its 2 neighbors makes 3 obstacles
           ASSERT_EQ(  
           countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ) +
           countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   51u );
          
           // @todo Rewrite
           // Add an obstacle at <2,  0> which will inflate and refresh to of the other inflated cells
           addObservation(  olayer,   2,   0 );
           layers.updateMap(  0,   0,   0 );
          
           // Now we expect insertions for it,   and 2 more neighbors,   but not all 5.
           // Free space will propagate from
           // the origin to the target,   clearing the point at <0,   0>,  
           // but not over-writing the inflation of the obstacle
           // at <0,   1>
           ASSERT_EQ(  
           countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ) +
           countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   54u );
          
           // Add an obstacle at <1,   9>. This will inflate obstacles around it
           addObservation(  olayer,   1,   9 );
           layers.updateMap(  0,   0,   0 );
          
           ASSERT_EQ(  costmap->getCost(  1,   9 ),   nav2_costmap_2d::LETHAL_OBSTACLE );
           ASSERT_EQ(  costmap->getCost(  0,   9 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
           ASSERT_EQ(  costmap->getCost(  2,   9 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
          
           // Add an obstacle and verify that it over-writes its inflated status
           addObservation(  olayer,   0,   9 );
           layers.updateMap(  0,   0,   0 );
          
           ASSERT_EQ(  costmap->getCost(  0,   9 ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          }
          
          /**
           * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
           */
     526  TEST_F(  TestNode,   testInflation2 )
          {
           initNode(  1 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           // Footprint with inscribed radius = 2.1
           // circumscribed radius = 3.1
           std::vector<Point> polygon = setRadii(  layers,   1,   1 );
          
           std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
           addStaticLayer(  layers,   tf,   node_,   slayer );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           waitForMap(  slayer );
          
           // Creat a small L-Shape all at once
           addObservation(  olayer,   1,   1,   MAX_Z );
           addObservation(  olayer,   2,   1,   MAX_Z );
           addObservation(  olayer,   2,   2,   MAX_Z );
           layers.updateMap(  0,   0,   0 );
          
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           // printMap(  *costmap );
           ASSERT_EQ(  costmap->getCost(  2,   3 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
           ASSERT_EQ(  costmap->getCost(  3,   3 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
          }
          
          /**
           * Test inflation behavior,   starting with an empty map
           */
     564  TEST_F(  TestNode,   testInflation3 )
          {
           initNode(  3 );
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           // 1 2 3
           std::vector<Point> polygon = setRadii(  layers,   1,   1.75 );
          
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  layers,   tf,   node_,   ilayer );
          
           layers.setFootprint(  polygon );
          
           // There should be no occupied cells
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   0u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   0u );
           printMap(  *costmap );
           // Add an obstacle at 5,  5
           addObservation(  olayer,   5,   5,   MAX_Z );
           layers.updateMap(  0,   0,   0 );
           printMap(  *costmap );
          
           // Test fails because updated cell value is 0
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::FREE_SPACE,   false ),   29u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   1u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   4u );
          
           // Update again - should see no change
           layers.updateMap(  0,   0,   0 );
          
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::FREE_SPACE,   false ),   29u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   1u );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ),   4u );
          }
          
          /**
           * Test dynamic parameter setting of inflation layer
           */
     608  TEST_F(  TestNode,   testDynParamsSet )
          {
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           costmap->set_parameter(  rclcpp::Parameter(  "global_frame",   std::string(  "base_link" ) ) );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
          
           costmap->on_activate(  rclcpp_lifecycle::State(   ) );
          
           auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           costmap->get_node_base_interface(   ),   costmap->get_node_topics_interface(   ),  
           costmap->get_node_graph_interface(   ),  
           costmap->get_node_services_interface(   ) );
          
           auto results = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "inflation_layer.inflation_radius",   0.0 ),  
           rclcpp::Parameter(  "inflation_layer.cost_scaling_factor",   0.0 ),  
           rclcpp::Parameter(  "inflation_layer.inflate_unknown",   true ),  
           rclcpp::Parameter(  "inflation_layer.inflate_around_unknown",   true ),  
           rclcpp::Parameter(  "inflation_layer.enabled",   false )
           } );
          
           rclcpp::spin_until_future_complete(  
           costmap->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  costmap->get_parameter(  "inflation_layer.inflation_radius" ).as_double(   ),   0.0 );
           EXPECT_EQ(  costmap->get_parameter(  "inflation_layer.cost_scaling_factor" ).as_double(   ),   0.0 );
           EXPECT_EQ(  costmap->get_parameter(  "inflation_layer.inflate_unknown" ).as_bool(   ),   true );
           EXPECT_EQ(  costmap->get_parameter(  "inflation_layer.inflate_around_unknown" ).as_bool(   ),   true );
           EXPECT_EQ(  costmap->get_parameter(  "inflation_layer.enabled" ).as_bool(   ),   false );
          
           costmap->on_deactivate(  rclcpp_lifecycle::State(   ) );
           costmap->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap->on_shutdown(  rclcpp_lifecycle::State(   ) );
          }

navigation2/nav2_costmap_2d/test/integration/obstacle_tests.cpp

       1  /*
           * Copyright (  c ) 2013,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          /**
           * @author David Lu!!
           * Test harness for ObstacleLayer for Costmap2D
           */
          
          #include <memory>
          #include <string>
          #include <algorithm>
          #include <utility>
          
          #include "gtest/gtest.h"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/observation_buffer.hpp"
          #include "../testing_helper.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          using std::begin;
          using std::end;
          using std::for_each;
          using std::all_of;
          using std::none_of;
          using std::pair;
          using std::string;
          
      55  class RclCppFixture
          {
          public:
      58   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      59   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      63  class TestLifecycleNode : public nav2_util::LifecycleNode
          {
          public:
      66   explicit TestLifecycleNode(  const string & name )
           : nav2_util::LifecycleNode(  name )
           {
           }
          
      71   nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      76   nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      81   nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      86   nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      91   nav2_util::CallbackReturn onShutdown(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      96   nav2_util::CallbackReturn onError(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          };
          
     102  class TestNode : public ::testing::Test
          {
          public:
     105   TestNode(   )
           {
           node_ = std::make_shared<TestLifecycleNode>(  "obstacle_test_node" );
           node_->declare_parameter(  "map_topic",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
           node_->declare_parameter(  "track_unknown_space",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "use_maximum",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "lethal_cost_threshold",   rclcpp::ParameterValue(  100 ) );
           node_->declare_parameter(  
           "unknown_cost_value",  
           rclcpp::ParameterValue(  static_cast<unsigned char>(  0xff ) ) );
           node_->declare_parameter(  "trinary_costmap",   rclcpp::ParameterValue(  true ) );
           node_->declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.3 ) );
           node_->declare_parameter(  "observation_sources",   rclcpp::ParameterValue(  std::string(  "" ) ) );
           }
          
     120   ~TestNode(   ) {}
          
          protected:
     123   std::shared_ptr<TestLifecycleNode> node_;
          };
          
          /*
           * For reference,   the static map looks like this:
           *
           * 0 0 0 0 0 0 0 254 254 254
           *
           * 0 0 0 0 0 0 0 254 254 254
           *
           * 0 0 0 254 254 254 0 0 0 0
           *
           * 0 0 0 0 0 0 0 0 0 0
           *
           * 0 0 0 0 0 0 0 0 0 0
           *
           * 0 0 0 0 254 0 0 254 254 254
           *
           * 0 0 0 0 254 0 0 254 254 254
           *
           * 0 0 0 0 0 0 0 254 254 254
           *
           * 0 0 0 0 0 0 0 0 0 0
           *
           * 0 0 0 0 0 0 0 0 0 0
           *
           * upper left is 0,  0,   lower right is 9,  9
           */
          
          #if (  0 )
          /**
           * Test for ray tracing free space
           */
     156  TEST_F(  TestNode,   testRaytracing ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
          
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           addStaticLayer(  layers,   tf,   node_ );
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // Add a point at 0,   0,   0
           addObservation(  olayer,   0.0,   0.0,   MAX_Z / 2,   0,   0,   MAX_Z / 2 );
          
           // This actually puts the LETHAL (  254 ) point in the costmap at (  0,  0 )
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           int lethal_count = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           // We expect just one obstacle to be added (  20 in static map )
           ASSERT_EQ(  lethal_count,   21 );
          }
          
          /**
           * Test for ray tracing free space
           */
     179  TEST_F(  TestNode,   testRaytracing2 ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           addStaticLayer(  layers,   tf,   node_ );
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // If we print map now,   it is 10x10 all value 0
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           // Update will fill in the costmap with the static map
           layers.updateMap(  0,   0,   0 );
          
           // If we print the map now,   we get the static map
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           // Static map has 20 LETHAL cells (  see diagram above )
           int obs_before = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
           ASSERT_EQ(  obs_before,   20 );
          
           // The sensor origin will be <0,  0>. So if we add an obstacle at 9,  9,  
           // we would expect cells <0,   0> thru <8,   8> to be traced through
           // however the static map is not cleared by obstacle layer
           addObservation(  olayer,   9.5,   9.5,   MAX_Z / 2,   0.5,   0.5,   MAX_Z / 2 );
           layers.updateMap(  0,   0,   0 );
          
           // If we print map now,   we have static map + <9,  9> is LETHAL
           // printMap(  *(  layers.getCostmap(   ) ) );
           int obs_after = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           // Change from previous test:
           // No obstacles from the static map will be cleared,   so the
           // net change is +1.
           ASSERT_EQ(  obs_after,   obs_before + 1 );
          
           // Fill in the diagonal,   <7,  7> and <9,  9> already filled in,   <0,  0> is robot
           for (  int i = 0; i < olayer->getSizeInCellsY(   ); ++i ) {
           olayer->setCost(  i,   i,   nav2_costmap_2d::LETHAL_OBSTACLE );
           }
           // This will updateBounds,   which will raytrace the static observation added
           // above,   thus clearing out the diagonal again!
           layers.updateMap(  0,   0,   0 );
          
           // Map now has diagonal except <0,  0> filled with LETHAL (  254 )
           // printMap(  *(  layers.getCostmap(   ) ) );
           int with_static = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           // Should thus be the same
           ASSERT_EQ(  with_static,   obs_after );
           // If 21 are filled,   79 should be free
           ASSERT_EQ(  79,   countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::FREE_SPACE ) );
          }
          
          /**
           * Test for wave interference
           */
     234  TEST_F(  TestNode,   testWaveInterference ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           node_->set_parameter(  rclcpp::Parameter(  "track_unknown_space",   true ) );
           // Start with an empty map,   no rolling window,   tracking unknown
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   true );
           layers.resizeMap(  10,   10,   1,   0,   0 );
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // If we print map now,   it is 10x10,   all cells are 255 (  NO_INFORMATION )
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           // Lay out 3 obstacles in a line - along the diagonal,   separated by a cell.
           addObservation(  olayer,   3.0,   3.0,   MAX_Z );
           addObservation(  olayer,   5.0,   5.0,   MAX_Z );
           addObservation(  olayer,   7.0,   7.0,   MAX_Z );
           layers.updateMap(  0,   0,   0 );
          
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           // 3 obstacle cells are filled,   <1,  1>,  <2,  2>,  <4,  4> and <6,  6> are now free
           // <0,  0> is footprint and is free
           // printMap(  *costmap );
           ASSERT_EQ(  3,   countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ) );
           ASSERT_EQ(  92,   countValues(  *costmap,   nav2_costmap_2d::NO_INFORMATION ) );
           ASSERT_EQ(  5,   countValues(  *costmap,   nav2_costmap_2d::FREE_SPACE ) );
          }
          
          /**
           * Make sure we ignore points outside of our z threshold
           */
     263  TEST_F(  TestNode,   testZThreshold ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           // Start with an empty map
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   true );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // A point cloud with 2 points falling in a cell with a non-lethal cost
           addObservation(  olayer,   0.0,   5.0,   0.4 );
           addObservation(  olayer,   1.0,   5.0,   2.2 );
          
           layers.updateMap(  0,   0,   0 );
          
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   1 );
          }
          
          /**
           * Verify that dynamic obstacles are added
           */
     284  TEST_F(  TestNode,   testDynamicObstacles ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           addStaticLayer(  layers,   tf,   node_ );
          
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // Add a point cloud and verify its insertion. There should be only one new one
           addObservation(  olayer,   0.0,   0.0 );
           addObservation(  olayer,   0.0,   0.0 );
           addObservation(  olayer,   0.0,   0.0 );
          
           layers.updateMap(  0,   0,   0 );
          
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           // Should now have 1 insertion and no deletions
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   21 );
          
           // Repeating the call - we should see no insertions or deletions
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   21 );
          }
          
          /**
           * Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
           */
     309  TEST_F(  TestNode,   testMultipleAdditions ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           addStaticLayer(  layers,   tf,   node_ );
          
           auto olayer = addObstacleLayer(  layers,   tf,   node_ );
          
           // A point cloud with one point that falls within an existing obstacle
           addObservation(  olayer,   9.5,   0.0 );
           layers.updateMap(  0,   0,   0 );
           nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(   );
           // printMap(  *costmap );
          
           ASSERT_EQ(  countValues(  *costmap,   nav2_costmap_2d::LETHAL_OBSTACLE ),   20 );
          }
          #endif
          /**
           * Verify correct init/reset cycling of layer
           */
     328  TEST_F(  TestNode,   testRepeatedResets ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
           addStaticLayer(  layers,   tf,   node_,   slayer );
          
           // TODO(  orduno ) Add obstacle layer
          
           // Define a node-level parameter
           pair<string,   string> node_dummy = {"node_dummy_param",   "node_dummy_val"};
           node_->declare_parameter(  node_dummy.first,   rclcpp::ParameterValue(  node_dummy.second ) );
          
           // Define a layer-level parameter
           pair<string,   string> layer_dummy = {"dummy_param",   "dummy_val"};
          
           // Set parameters
           auto plugins = layers.getPlugins(   );
           for_each(  
           begin(  *plugins ),   end(  *plugins ),   [&layer_dummy](  const auto & plugin ) {
           string layer_param = layer_dummy.first + "_" + plugin->getName(   );
          
           // Notice we are using Layer::declareParameter
           plugin->declareParameter(  layer_param,   rclcpp::ParameterValue(  layer_dummy.second ) );
           } );
          
           // Check that all parameters have been set
           // node-level param
           ASSERT_TRUE(  node_->has_parameter(  node_dummy.first ) );
          
           // layer-level param
           ASSERT_TRUE(  
           all_of(  
           begin(  *plugins ),   end(  *plugins ),   [&layer_dummy](  const auto & plugin ) {
           string layer_param = layer_dummy.first + "_" + plugin->getName(   );
           return plugin->hasParameter(  layer_param );
           } ) );
          
           // Reset all layers. Parameters should be declared if not declared,   otherwise skipped.
           ASSERT_NO_THROW(  
           for_each(  
           begin(  *plugins ),   end(  *plugins ),   [](  const auto & plugin ) {
           plugin->reset(   );
           } ) );
          }
          
          
          /**
           * Test for ray tracing free space
           */
     378  TEST_F(  TestNode,   testRaytracing ) {
           tf2_ros::Buffer tf(  node_->get_clock(   ) );
          
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
           addStaticLayer(  layers,   tf,   node_,   slayer );
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
           addObstacleLayer(  layers,   tf,   node_,   olayer );
          
           addObservation(  olayer,   0.0,   0.0,   MAX_Z / 2,   0,   0,   MAX_Z / 2 );
          
           // This actually puts the LETHAL (  254 ) point in the costmap at (  0,  0 )
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           int lethal_count = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           ASSERT_EQ(  lethal_count,   1 );
          
           addObservation(  olayer,   1.0,   1.0,   MAX_Z / 2,   0,   0,   MAX_Z / 2,   true,   true,   100.0,   5.0,   100.0,   5.0 );
          
           // This actually puts the LETHAL (  254 ) point in the costmap at (  0,  0 )
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
           // printMap(  *(  layers.getCostmap(   ) ) );
          
           // New observation should not be recorded as min_range is higher than obstacle range
           lethal_count = countValues(  *(  layers.getCostmap(   ) ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           ASSERT_EQ(  lethal_count,   1 );
          }
          
          /**
           * Test dynamic parameter setting of obstacle layer
           */
     414  TEST_F(  TestNode,   testDynParamsSetObstacle )
          {
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           // Add obstacle layer
           std::vector<std::string> plugins_str;
           plugins_str.push_back(  "obstacle_layer" );
           costmap->set_parameter(  rclcpp::Parameter(  "plugins",   plugins_str ) );
           costmap->declare_parameter(  
           "obstacle_layer.plugin",  
           rclcpp::ParameterValue(  std::string(  "nav2_costmap_2d::ObstacleLayer" ) ) );
          
           costmap->set_parameter(  rclcpp::Parameter(  "global_frame",   std::string(  "base_link" ) ) );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
          
           costmap->on_activate(  rclcpp_lifecycle::State(   ) );
          
           auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           costmap->get_node_base_interface(   ),   costmap->get_node_topics_interface(   ),  
           costmap->get_node_graph_interface(   ),  
           costmap->get_node_services_interface(   ) );
          
           auto results = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "obstacle_layer.combination_method",   5 ),  
           rclcpp::Parameter(  "obstacle_layer.max_obstacle_height",   4.0 ),  
           rclcpp::Parameter(  "obstacle_layer.enabled",   false ),  
           rclcpp::Parameter(  "obstacle_layer.footprint_clearing_enabled",   false )
           } );
          
           rclcpp::spin_until_future_complete(  
           costmap->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  costmap->get_parameter(  "obstacle_layer.combination_method" ).as_int(   ),   5 );
           EXPECT_EQ(  costmap->get_parameter(  "obstacle_layer.max_obstacle_height" ).as_double(   ),   4.0 );
           EXPECT_EQ(  costmap->get_parameter(  "obstacle_layer.enabled" ).as_bool(   ),   false );
           EXPECT_EQ(  costmap->get_parameter(  "obstacle_layer.footprint_clearing_enabled" ).as_bool(   ),   false );
          
           costmap->on_deactivate(  rclcpp_lifecycle::State(   ) );
           costmap->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap->on_shutdown(  rclcpp_lifecycle::State(   ) );
          }
          
          /**
           * Test dynamic parameter setting of voxel layer
           */
     461  TEST_F(  TestNode,   testDynParamsSetVoxel )
          {
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           // Add voxel layer
           std::vector<std::string> plugins_str;
           plugins_str.push_back(  "voxel_layer" );
           costmap->set_parameter(  rclcpp::Parameter(  "plugins",   plugins_str ) );
           costmap->declare_parameter(  
           "voxel_layer.plugin",  
           rclcpp::ParameterValue(  std::string(  "nav2_costmap_2d::VoxelLayer" ) ) );
          
           costmap->set_parameter(  rclcpp::Parameter(  "global_frame",   std::string(  "base_link" ) ) );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
          
           costmap->on_activate(  rclcpp_lifecycle::State(   ) );
          
           auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           costmap->get_node_base_interface(   ),   costmap->get_node_topics_interface(   ),  
           costmap->get_node_graph_interface(   ),  
           costmap->get_node_services_interface(   ) );
          
           auto results = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "voxel_layer.combination_method",   0 ),  
           rclcpp::Parameter(  "voxel_layer.mark_threshold",   1 ),  
           rclcpp::Parameter(  "voxel_layer.unknown_threshold",   10 ),  
           rclcpp::Parameter(  "voxel_layer.z_resolution",   0.4 ),  
           rclcpp::Parameter(  "voxel_layer.origin_z",   1.0 ),  
           rclcpp::Parameter(  "voxel_layer.z_voxels",   14 ),  
           rclcpp::Parameter(  "voxel_layer.max_obstacle_height",   4.0 ),  
           rclcpp::Parameter(  "voxel_layer.footprint_clearing_enabled",   false ),  
           rclcpp::Parameter(  "voxel_layer.enabled",   false ),  
           rclcpp::Parameter(  "voxel_layer.publish_voxel_map",   true )
           } );
          
           rclcpp::spin_until_future_complete(  
           costmap->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.combination_method" ).as_int(   ),   0 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.mark_threshold" ).as_int(   ),   1 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.unknown_threshold" ).as_int(   ),   10 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.z_resolution" ).as_double(   ),   0.4 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.origin_z" ).as_double(   ),   1.0 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.z_voxels" ).as_int(   ),   14 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.max_obstacle_height" ).as_double(   ),   4.0 );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.footprint_clearing_enabled" ).as_bool(   ),   false );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.enabled" ).as_bool(   ),   false );
           EXPECT_EQ(  costmap->get_parameter(  "voxel_layer.publish_voxel_map" ).as_bool(   ),   true );
          
           costmap->on_deactivate(  rclcpp_lifecycle::State(   ) );
           costmap->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap->on_shutdown(  rclcpp_lifecycle::State(   ) );
          }
          
          /**
           * Test dynamic parameter setting of static layer
           */
     520  TEST_F(  TestNode,   testDynParamsSetStatic )
          {
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_costmap" );
          
           costmap->set_parameter(  rclcpp::Parameter(  "global_frame",   std::string(  "base_link" ) ) );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
          
           costmap->on_activate(  rclcpp_lifecycle::State(   ) );
          
           auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(  
           costmap->get_node_base_interface(   ),   costmap->get_node_topics_interface(   ),  
           costmap->get_node_graph_interface(   ),  
           costmap->get_node_services_interface(   ) );
          
           auto results = parameter_client->set_parameters_atomically(  
           {
           rclcpp::Parameter(  "static_layer.transform_tolerance",   1.0 ),  
           rclcpp::Parameter(  "static_layer.enabled",   false ),  
           rclcpp::Parameter(  "static_layer.map_subscribe_transient_local",   false ),  
           rclcpp::Parameter(  "static_layer.map_topic",   "dynamic_topic" ),  
           rclcpp::Parameter(  "static_layer.subscribe_to_updates",   true )
           } );
          
           rclcpp::spin_until_future_complete(  
           costmap->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  costmap->get_parameter(  "static_layer.transform_tolerance" ).as_double(   ),   1.0 );
           EXPECT_EQ(  costmap->get_parameter(  "static_layer.enabled" ).as_bool(   ),   false );
           EXPECT_EQ(  costmap->get_parameter(  "static_layer.map_subscribe_transient_local" ).as_bool(   ),   false );
           EXPECT_EQ(  costmap->get_parameter(  "static_layer.map_topic" ).as_string(   ),   "dynamic_topic" );
           EXPECT_EQ(  costmap->get_parameter(  "static_layer.subscribe_to_updates" ).as_bool(   ),   true );
          
           costmap->on_deactivate(  rclcpp_lifecycle::State(   ) );
           costmap->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap->on_shutdown(  rclcpp_lifecycle::State(   ) );
          }

navigation2/nav2_costmap_2d/test/integration/range_tests.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020,   Bytes Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          #include <algorithm>
          #include <utility>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/observation_buffer.hpp"
          #include "../testing_helper.hpp"
          #include "sensor_msgs/msg/range.hpp"
          
          using std::begin;
          using std::end;
          using std::for_each;
          using std::all_of;
          using std::none_of;
          using std::pair;
          using std::string;
          
      56  class RclCppFixture
          {
          public:
      59   RclCppFixture(   )
           {
           rclcpp::init(  0,   nullptr );
           }
          
      64   ~RclCppFixture(   )
           {
           rclcpp::shutdown(   );
           }
          };
          
          RclCppFixture g_rclcppfixture;
          
      72  class TestLifecycleNode : public nav2_util::LifecycleNode
          {
          public:
      75   explicit TestLifecycleNode(  const string & name )
           : nav2_util::LifecycleNode(  name )
           {
           }
          
      80   nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      85   nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      90   nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      95   nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     100   nav2_util::CallbackReturn onShutdown(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     105   nav2_util::CallbackReturn onError(  const rclcpp_lifecycle::State & )
           {
           return nav2_util::CallbackReturn::SUCCESS;
           }
          };
          
     111  class TestNode : public ::testing::Test
          {
          public:
     114   TestNode(   )
           : node_(  std::make_shared<TestLifecycleNode>(  "range_test_node" ) ),  
           tf_(  node_->get_clock(   ) )
           {
           tf_.setUsingDedicatedThread(  true );
           // Standard non-plugin specific parameters
           node_->declare_parameter(  "map_topic",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
           node_->declare_parameter(  "track_unknown_space",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "use_maximum",   rclcpp::ParameterValue(  false ) );
           node_->declare_parameter(  "lethal_cost_threshold",   rclcpp::ParameterValue(  100 ) );
           node_->declare_parameter(  
           "unknown_cost_value",  
           rclcpp::ParameterValue(  static_cast<unsigned char>(  0xff ) ) );
           node_->declare_parameter(  "trinary_costmap",   rclcpp::ParameterValue(  true ) );
           node_->declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.3 ) );
           node_->declare_parameter(  "observation_sources",   rclcpp::ParameterValue(  std::string(  "range" ) ) );
           node_->declare_parameter(  "global_frame",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
          
          
           // Range sensor specific parameters
           node_->declare_parameter(  
           "range.topics",  
           rclcpp::ParameterValue(  
           std::vector<std::string>{"/range/topic"} ) );
           node_->declare_parameter(  "range.phi",   rclcpp::ParameterValue(  1.2 ) );
           node_->declare_parameter(  "range.clear_on_max_reading",   rclcpp::ParameterValue(  true ) );
           }
          
     142   ~TestNode(   ) {}
          
          protected:
     145   std::shared_ptr<TestLifecycleNode> node_;
     146   tf2_ros::Buffer tf_;
          };
          
          // Test clearing at max range
     150  TEST_F(  TestNode,   testClearingAtMaxRange ) {
           geometry_msgs::msg::TransformStamped transform;
           transform.header.stamp = node_->now(   );
           transform.header.frame_id = "frame";
           transform.child_frame_id = "base_link";
           transform.transform.translation.y = 5;
           transform.transform.translation.x = 2;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
           addRangeLayer(  layers,   tf_,   node_,   rlayer );
          
           sensor_msgs::msg::Range msg;
           msg.min_range = 1.0;
           msg.max_range = 7.0;
           msg.range = 2.0;
           msg.header.stamp = node_->now(   );
           msg.header.frame_id = "base_link";
           msg.radiation_type = msg.ULTRASOUND;
           msg.field_of_view = 0.174533; // 10 deg
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  4,   5 ),   254 );
          
           msg.range = 7.0;
           msg.header.stamp = node_->now(   );
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  4,   5 ),   0 );
          }
          
          // Testing fixed scan with robot forward motion
     190  TEST_F(  TestNode,   testProbabalisticModelForward ) {
           geometry_msgs::msg::TransformStamped transform;
           transform.header.stamp = node_->now(   );
           transform.header.frame_id = "frame";
           transform.child_frame_id = "base_link";
           transform.transform.translation.y = 5;
           transform.transform.translation.x = 2;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
           addRangeLayer(  layers,   tf_,   node_,   rlayer );
          
           sensor_msgs::msg::Range msg;
           msg.min_range = 1.0;
           msg.max_range = 10.0;
           msg.range = 3.0;
           msg.header.stamp = node_->now(   );
           msg.header.frame_id = "base_link";
           msg.radiation_type = msg.ULTRASOUND;
           msg.field_of_view = 0.174533; // 10 deg
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
           transform.transform.translation.y = 5;
           transform.transform.translation.x = 4;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           transform.transform.translation.y = 5;
           transform.transform.translation.x = 6;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  5,   5 ),   254 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  6,   5 ),   0 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  7,   5 ),   254 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  8,   5 ),   0 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  9,   5 ),   254 );
          }
          
          // Testing fixed motion with downward movement
     243  TEST_F(  TestNode,   testProbabalisticModelDownward ) {
           geometry_msgs::msg::TransformStamped transform;
           transform.header.stamp = node_->now(   );
           transform.header.frame_id = "frame";
           transform.child_frame_id = "base_link";
           transform.transform.translation.y = 3;
           transform.transform.translation.x = 2;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
           layers.resizeMap(  10,   10,   1,   0,   0 );
          
           std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
           addRangeLayer(  layers,   tf_,   node_,   rlayer );
          
           sensor_msgs::msg::Range msg;
           msg.min_range = 1.0;
           msg.max_range = 10.0;
           msg.range = 1.0;
           msg.header.stamp = node_->now(   );
           msg.header.frame_id = "base_link";
           msg.radiation_type = msg.ULTRASOUND;
           msg.field_of_view = 0.174533; // 10 deg
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           transform.transform.translation.y = 5;
           transform.transform.translation.x = 2;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           rlayer->bufferIncomingRangeMsg(  std::make_shared<sensor_msgs::msg::Range>(  msg ) );
          
           transform.transform.translation.y = 7;
           transform.transform.translation.x = 2;
           tf_.setTransform(  transform,   "default_authority",   true );
          
           layers.updateMap(  0,   0,   0 ); // 0,   0,   0 is robot pose
          // printMap(  *(  layers.getCostmap(   ) ) );
          
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  3,   3 ),   254 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  3,   4 ),   0 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  3,   5 ),   254 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  3,   6 ),   0 );
           ASSERT_EQ(  layers.getCostmap(   )->getCost(  3,   7 ),   254 );
          }

navigation2/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <chrono>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/layered_costmap.hpp"
          #include "nav2_costmap_2d/static_layer.hpp"
          #include "nav2_costmap_2d/inflation_layer.hpp"
          #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
          #include "../testing_helper.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          #include "tf2_ros/transform_broadcaster.h"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          #include "nav2_util/geometry_utils.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::placeholders;
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
      46  class RclCppFixture
          {
          public:
      49   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      50   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      54  class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
          {
          public:
      57   DummyCostmapSubscriber(  
      58   nav2_util::LifecycleNode::SharedPtr node,  
      59   std::string & topic_name )
           : CostmapSubscriber(  node,   topic_name )
           {}
          
      63   void setCostmap(  nav2_msgs::msg::Costmap::SharedPtr msg )
           {
           costmap_msg_ = msg;
           costmap_received_ = true;
           }
          };
          
      70  class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
          {
          public:
      73   DummyFootprintSubscriber(  
      74   nav2_util::LifecycleNode::SharedPtr node,  
      75   std::string & topic_name,  
      76   tf2_ros::Buffer & tf_ )
           : FootprintSubscriber(  node,   topic_name,   tf_ )
           {}
          
      80   void setFootprint(  geometry_msgs::msg::PolygonStamped::SharedPtr msg )
           {
           footprint_ = msg;
           footprint_received_ = true;
           }
          };
          
      87  class TestCollisionChecker : public nav2_util::LifecycleNode
          {
          public:
      90   explicit TestCollisionChecker(  std::string name )
           : LifecycleNode(  name ),  
           global_frame_(  "map" )
           {
           // Declare non-plugin specific costmap parameters
           declare_parameter(  "map_topic",   rclcpp::ParameterValue(  std::string(  "map" ) ) );
           declare_parameter(  "track_unknown_space",   rclcpp::ParameterValue(  true ) );
           declare_parameter(  "use_maximum",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "lethal_cost_threshold",   rclcpp::ParameterValue(  100 ) );
           declare_parameter(  
           "unknown_cost_value",  
           rclcpp::ParameterValue(  static_cast<unsigned char>(  0xff ) ) );
           declare_parameter(  "trinary_costmap",   rclcpp::ParameterValue(  true ) );
           }
          
           nav2_util::CallbackReturn
     106   on_configure(  const rclcpp_lifecycle::State & /*state*/ )
           {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
           callback_group_ = create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,   false );
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),  
           get_node_timers_interface(   ),  
           callback_group_ );
           tf_buffer_->setCreateTimerInterface(  timer_interface );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
           tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(  shared_from_this(   ) );
          
           std::string costmap_topic = "costmap_raw";
           std::string footprint_topic = "published_footprint";
          
           costmap_sub_ = std::make_shared<DummyCostmapSubscriber>(  
           shared_from_this(   ),  
           costmap_topic );
          
           footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(  
           shared_from_this(   ),  
           footprint_topic,  
           *tf_buffer_ );
          
           collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(  
           *costmap_sub_,   *footprint_sub_,   get_name(   ) );
          
           layers_ = new nav2_costmap_2d::LayeredCostmap(  "map",   false,   false );
           // Add Static Layer
           std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
           addStaticLayer(  *layers_,   *tf_buffer_,   shared_from_this(   ),   slayer,   callback_group_ );
          
           while (  !slayer->isCurrent(   ) ) {
           rclcpp::spin_some(  this->get_node_base_interface(   ) );
           }
           // Add Inflation Layer
           std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
           addInflationLayer(  *layers_,   *tf_buffer_,   shared_from_this(   ),   ilayer,   callback_group_ );
          
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           executor_->add_callback_group(  callback_group_,   get_node_base_interface(   ) );
           executor_thread_ = std::make_unique<nav2_util::NodeThread>(  executor_ );
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
           nav2_util::CallbackReturn
     154   on_activate(  const rclcpp_lifecycle::State & /*state*/ )
           {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
           nav2_util::CallbackReturn
     161   on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
           {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
           nav2_util::CallbackReturn
     168   on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
           {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning Up" );
           delete layers_;
           layers_ = nullptr;
          
           executor_thread_.reset(   );
           tf_buffer_.reset(   );
          
           footprint_sub_.reset(   );
           costmap_sub_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
     183   ~TestCollisionChecker(   ) {}
          
     185   bool testPose(  double x,   double y,   double theta )
           {
           rclcpp::Time stamp = now(   );
           publishPose(  x,   y,   theta,   stamp );
           geometry_msgs::msg::Pose2D pose;
           pose.x = x;
           pose.y = y;
           pose.theta = theta;
          
           setPose(  x,   y,   theta,   stamp );
           publishFootprint(   );
           publishCostmap(   );
           rclcpp::sleep_for(  std::chrono::milliseconds(  1000 ) );
           return collision_checker_->isCollisionFree(  pose );
           }
          
     201   void setFootprint(  double footprint_padding,   double robot_radius )
           {
           std::vector<geometry_msgs::msg::Point> new_footprint;
           new_footprint = nav2_costmap_2d::makeFootprintFromRadius(  robot_radius );
           nav2_costmap_2d::padFootprint(  new_footprint,   footprint_padding );
           footprint_ = new_footprint;
           layers_->setFootprint(  footprint_ );
           }
          
          protected:
     211   void setPose(  double x,   double y,   double theta,   const rclcpp::Time & stamp )
           {
           x_ = x;
           y_ = y;
           yaw_ = theta;
           stamp_ = stamp;
          
           current_pose_.pose.position.x = x_;
           current_pose_.pose.position.y = y_;
           current_pose_.pose.position.z = 0;
           current_pose_.pose.orientation = orientationAroundZAxis(  yaw_ );
           current_pose_.header.stamp = stamp;
           }
          
     225   void publishFootprint(   )
           {
           geometry_msgs::msg::PolygonStamped oriented_footprint;
           oriented_footprint.header.frame_id = global_frame_;
           oriented_footprint.header.stamp = stamp_;
           nav2_costmap_2d::transformFootprint(  x_,   y_,   yaw_,   footprint_,   oriented_footprint );
           footprint_sub_->setFootprint(  
           std::make_shared<geometry_msgs::msg::PolygonStamped>(  oriented_footprint ) );
           }
          
     235   void publishCostmap(   )
           {
           layers_->updateMap(  x_,   y_,   yaw_ );
           costmap_sub_->setCostmap(  
           std::make_shared<nav2_msgs::msg::Costmap>(  toCostmapMsg(  layers_->getCostmap(   ) ) ) );
           }
          
     242   void publishPose(  double x,   double y,   double /*theta*/,   const rclcpp::Time & stamp )
           {
           geometry_msgs::msg::TransformStamped tf_stamped;
           tf_stamped.header.frame_id = "map";
           tf_stamped.header.stamp = stamp;
           tf_stamped.child_frame_id = "base_link";
           tf_stamped.transform.translation.x = x;
           tf_stamped.transform.translation.y = y;
           tf_stamped.transform.rotation.w = 1.0;
           tf_broadcaster_->sendTransform(  tf_stamped );
           }
          
           nav2_msgs::msg::Costmap
     255   toCostmapMsg(  nav2_costmap_2d::Costmap2D * costmap )
           {
           double resolution = costmap->getResolution(   );
          
           double wx,   wy;
           costmap->mapToWorld(  0,   0,   wx,   wy );
          
           unsigned char * data = costmap->getCharMap(   );
          
           nav2_msgs::msg::Costmap costmap_msg;
           costmap_msg.header.frame_id = global_frame_;
           costmap_msg.header.stamp = stamp_;
           costmap_msg.metadata.layer = "master";
           costmap_msg.metadata.resolution = resolution;
           costmap_msg.metadata.size_x = costmap->getSizeInCellsX(   );
           costmap_msg.metadata.size_y = costmap->getSizeInCellsY(   );
           costmap_msg.metadata.origin.position.x = wx - resolution / 2;
           costmap_msg.metadata.origin.position.y = wy - resolution / 2;
           costmap_msg.metadata.origin.position.z = 0.0;
           costmap_msg.metadata.origin.orientation.w = 1.0;
           costmap_msg.data.resize(  costmap_msg.metadata.size_x * costmap_msg.metadata.size_y );
          
           for (  unsigned int i = 0; i < costmap_msg.data.size(   ); i++ ) {
           costmap_msg.data[i] = data[i];
           }
          
           return costmap_msg;
           }
          
     284   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
     285   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
     286   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
          
     288   rclcpp::CallbackGroup::SharedPtr callback_group_;
     289   rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
     290   std::unique_ptr<nav2_util::NodeThread> executor_thread_;
          
     292   std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
     293   std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
     294   std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
          
           nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
           std::string global_frame_;
           double x_,   y_,   yaw_;
           rclcpp::Time stamp_;
           geometry_msgs::msg::PoseStamped current_pose_;
           std::vector<geometry_msgs::msg::Point> footprint_;
          };
          
          
          class TestNode : public ::testing::Test
          {
          public:
           TestNode(   )
           {
           collision_checker_ = std::make_shared<TestCollisionChecker>(  "test_collision_checker" );
           collision_checker_->on_configure(  collision_checker_->get_current_state(   ) );
           collision_checker_->on_activate(  collision_checker_->get_current_state(   ) );
           }
          
           ~TestNode(   )
           {
           collision_checker_->on_deactivate(  collision_checker_->get_current_state(   ) );
           collision_checker_->on_cleanup(  collision_checker_->get_current_state(   ) );
           }
          
          protected:
           std::shared_ptr<TestCollisionChecker> collision_checker_;
          };
          
          TEST_F(  TestNode,   unknownSpace )
          {
           collision_checker_->setFootprint(  0,   1 );
          
           // Completely off map
           ASSERT_EQ(  collision_checker_->testPose(  5,   13,   0 ),   false );
          
           // Partially off map
           ASSERT_EQ(  collision_checker_->testPose(  5,   9.5,   0 ),   false );
          
           // In unknown region inside map
           ASSERT_EQ(  collision_checker_->testPose(  2,   4,   0 ),   false );
          }
          
          TEST_F(  TestNode,   FreeSpace )
          {
           collision_checker_->setFootprint(  0,   1 );
          
           // In complete free space
           ASSERT_EQ(  collision_checker_->testPose(  2,   8.5,   0 ),   true );
          
           // Partially in inscribed space
           ASSERT_EQ(  collision_checker_->testPose(  2.5,   7,   0 ),   true );
          }
          
          TEST_F(  TestNode,   CollisionSpace )
          {
           collision_checker_->setFootprint(  0,   1 );
          
           // Completely in obstacle
           ASSERT_EQ(  collision_checker_->testPose(  8.5,   6.5,   0 ),   false );
          
           // Partially in obstacle
           ASSERT_EQ(  collision_checker_->testPose(  4.5,   4.5,   0 ),   false );
          }

navigation2/nav2_costmap_2d/test/module_tests.cpp

       1  /*
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          /**
           * @author Conor McGann
           * Test harness for Costmap2D
           */
          #include <gtest/gtest.h>
          #include <set>
          #include <vector>
          
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/observation_buffer.hpp"
          
          const unsigned char MAP_10_BY_10_CHAR[] = {
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   200,   200,   200,  
           0,   0,   0,   0,   100,   0,   0,   200,   200,   200,  
           0,   0,   0,   0,   100,   0,   0,   200,   200,   200,  
           70,   70,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   200,   200,   200,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   255,   255,   255,  
           0,   0,   0,   0,   0,   0,   0,   255,   255,   255
          };
          
          const unsigned char MAP_5_BY_5_CHAR[] = {
           0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,  
          };
          
          std::vector<unsigned char> MAP_5_BY_5;
          std::vector<unsigned char> MAP_10_BY_10;
          std::vector<unsigned char> EMPTY_10_BY_10;
          std::vector<unsigned char> EMPTY_100_BY_100;
          
      67  const unsigned int GRID_WIDTH(  10 );
      68  const unsigned int GRID_HEIGHT(  10 );
      69  const double RESOLUTION(  1 );
      70  const double WINDOW_LENGTH(  10 );
      71  const unsigned char THRESHOLD(  100 );
      72  const double MAX_Z(  1.0 );
      73  const double RAYTRACE_MAX_RANGE(  20.0 );
      74  const double RAYTRACE_MIN_RANGE(  3.0 );
      75  const double OBSTACLE_MAX_RANGE(  20.0 );
      76  const double OBSTACLE_MIN_RANGE(  0.0 );
      77  const double ROBOT_RADIUS(  1.0 );
          
      79  bool find(  const std::vector<unsigned int> & l,   unsigned int n )
          {
           for (  std::vector<unsigned int>::const_iterator it = l.begin(   ); it != l.end(   ); ++it ) {
           if (  *it == n ) {
           return true;
           }
           }
          
           return false;
          }
          
          /**
           * Tests the reset method
           */
      93  TEST(  costmap,   testResetForStaticMap ) {
           // Define a static map with a large object in the center
           std::vector<unsigned char> staticMap;
           for (  unsigned int i = 0; i < 10; i++ ) {
           for (  unsigned int j = 0; j < 10; j++ ) {
           staticMap.push_back(  nav2_costmap_2d::LETHAL_OBSTACLE );
           }
           }
          
           // Allocate the cost map,   with a inflation to 3 cells all around
           nav2_costmap_2d::Costmap2D map(  10,   10,   RESOLUTION,   0.0,   0.0,   3,   3,   3,  
           OBSTACLE_MAX_RANGE,   OBSTACLE_MIN_RANGE,   MAX_Z,   RAYTRACE_MAX_RANGE,   RAYTRACE_MIN_RANGE,   25,  
           staticMap,   THRESHOLD );
          
           // Populate the cost map with a wall around the perimeter. Free space should clear out the room.
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  40 );
          
           // Left wall
           unsigned int ind = 0;
           for (  unsigned int i = 0; i < 10; i++ ) {
           // Left
           cloud.points[ind].x = 0;
           cloud.points[ind].y = i;
           cloud.points[ind].z = MAX_Z;
           ind++;
          
           // Top
           cloud.points[ind].x = i;
           cloud.points[ind].y = 0;
           cloud.points[ind].z = MAX_Z;
           ind++;
          
           // Right
           cloud.points[ind].x = 9;
           cloud.points[ind].y = i;
           cloud.points[ind].z = MAX_Z;
           ind++;
          
           // Bottom
           cloud.points[ind].x = i;
           cloud.points[ind].y = 9;
           cloud.points[ind].z = MAX_Z;
           ind++;
           }
          
           double wx = 5.0,   wy = 5.0;
           geometry_msgs::Point p;
           p.x = wx;
           p.y = wy;
           p.z = MAX_Z;
           nav2_costmap_2d::Observation obs(  p,   cloud,   OBSTACLE_MAX_RANGE,   OBSTACLE_MIN_RANGE,  
           RAYTRACE_MAX_RANGE,  
           RAYTRACE_MIN_RANGE );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           // Update the cost map for this observation
           map.updateWorld(  wx,   wy,   obsBuf,   obsBuf );
          
           // Verify that we now have only 36 cells with lethal cost,  
           // thus provong that we have correctly cleared free space
           int hitCount = 0;
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           hitCount++;
           }
           }
           }
           ASSERT_EQ(  hitCount,   36 );
          
           // Veriy that we have 64 non-leathal
           hitCount = 0;
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) != nav2_costmap_2d::LETHAL_OBSTACLE ) {
           hitCount++;
           }
           }
           }
           ASSERT_EQ(  hitCount,   64 );
          
           // Now if we reset the cost map,   we should have our map go back to being completely occupied
           map.resetMapOutsideWindow(  wx,   wy,   0.0,   0.0 );
          
           // We should now go back to everything being occupied
           hitCount = 0;
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           hitCount++;
           }
           }
           }
           ASSERT_EQ(  hitCount,   100 );
          }
          
          /**
           * Test for the cost function correctness with a larger range and different values
           */
     194  TEST(  costmap,   testCostFunctionCorrectness ) {
           nav2_costmap_2d::Costmap2D map(  100,   100,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS * 5.0,   ROBOT_RADIUS * 8.0,   ROBOT_RADIUS * 10.5,  
           100.0,   MAX_Z,   100.0,   25,   EMPTY_100_BY_100,   THRESHOLD );
          
           // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
           unsigned char c = map.computeCost(  (  ROBOT_RADIUS * 8.0 / RESOLUTION ) );
           ASSERT_EQ(  map.getCircumscribedCost(   ),   c );
          
           // Add a point in the center
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  1 );
           cloud.points[0].x = 50;
           cloud.points[0].y = 50;
           cloud.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           for (  unsigned int i = 0; i <= (  unsigned int )ceil(  ROBOT_RADIUS * 5.0 ); i++ ) {
           // To the right
           ASSERT_EQ(  map.getCost(  50 + i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map.getCost(  50 + i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // To the left
           ASSERT_EQ(  map.getCost(  50 - i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map.getCost(  50 - i,   50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // Down
           ASSERT_EQ(  map.getCost(  50,   50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map.getCost(  50,   50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           // Up
           ASSERT_EQ(  map.getCost(  50,   50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           ASSERT_EQ(  map.getCost(  50,   50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           }
          
           // Verify the normalized cost attenuates as expected
           for (  unsigned int i = (  unsigned int )(  ceil(  ROBOT_RADIUS * 5.0 ) + 1 );
           i <= (  unsigned int )ceil(  ROBOT_RADIUS * 10.5 ); i++ )
           {
           unsigned char expectedValue = map.computeCost(  i / RESOLUTION );
           ASSERT_EQ(  map.getCost(  50 + i,   50 ),   expectedValue );
           }
          
           // Update with no hits. Should clear (  revert to the static map
           map.resetMapOutsideWindow(  0,   0,   0.0,   0.0 );
           cloud.points.resize(  0 );
          
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs2(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf2;
           obsBuf2.push_back(  obs2 );
          
           map.updateWorld(  0,   0,   obsBuf2,   obsBuf2 );
          
           for (  unsigned int i = 0; i < 100; i++ ) {
           for (  unsigned int j = 0; j < 100; j++ ) {
           ASSERT_EQ(  map.getCost(  i,   j ),   nav2_costmap_2d::FREE_SPACE );
           }
           }
          }
          
     265  char printableCost(  unsigned char cost )
          {
           switch (  cost ) {
           case nav2_costmap_2d::NO_INFORMATION: return '?';
           case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L';
           case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
           case nav2_costmap_2d::FREE_SPACE: return '.';
           default: return '0' + (  unsigned char ) (  10 * cost / 255 );
           }
          }
          
          /**
           * Test for wave interference
           */
     279  TEST(  costmap,   testWaveInterference ) {
           // Start with an empty map
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS * 2,   ROBOT_RADIUS * 3.01,  
           10.0,   MAX_Z * 2,   10.0,   1,   EMPTY_10_BY_10,   THRESHOLD );
          
           // Lay out 3 obstacles in a line - along the diagonal,   separated by a cell.
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  3 );
           cloud.points[0].x = 3;
           cloud.points[0].y = 3;
           cloud.points[0].z = MAX_Z;
           cloud.points[1].x = 5;
           cloud.points[1].y = 5;
           cloud.points[1].z = MAX_Z;
           cloud.points[2].x = 7;
           cloud.points[2].y = 7;
           cloud.points[2].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           int update_count = 0;
          
           // Expect to see a union of obstacles
           printf(  "map:\n" );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) != nav2_costmap_2d::FREE_SPACE ) {
           update_count++;
           }
           printf(  "%c",   printableCost(  map.getCost(  i,   j ) ) );
           }
           printf(  "\n" );
           }
          
           ASSERT_EQ(  update_count,   79 );
          }
          
          /** Test for copying a window of a costmap */
     327  TEST(  costmap,   testWindowCopy ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           /*
           for(  unsigned int i = 0; i < 10; ++i ){
           for(  unsigned int j = 0; j < 10; ++j ){
           printf(  "%3d ",   map.getCost(  i,   j ) );
           }
           printf(  "\n" );
           }
           printf(  "\n" );
           */
          
           nav2_costmap_2d::Costmap2D windowCopy;
          
           // first test that if we try to make a window that is too big,   things fail
           windowCopy.copyCostmapWindow(  map,   2.0,   2.0,   6.0,   12.0 );
           ASSERT_EQ(  windowCopy.getSizeInCellsX(   ),   (  unsigned int )0 );
           ASSERT_EQ(  windowCopy.getSizeInCellsY(   ),   (  unsigned int )0 );
          
           // Next,   test that trying to make a map window itself fails
           map.copyCostmapWindow(  map,   2.0,   2.0,   6.0,   6.0 );
           ASSERT_EQ(  map.getSizeInCellsX(   ),   (  unsigned int )10 );
           ASSERT_EQ(  map.getSizeInCellsY(   ),   (  unsigned int )10 );
          
           // Next,   test that legal input generates the result that we expect
           windowCopy.copyCostmapWindow(  map,   2.0,   2.0,   6.0,   6.0 );
           ASSERT_EQ(  windowCopy.getSizeInCellsX(   ),   (  unsigned int )6 );
           ASSERT_EQ(  windowCopy.getSizeInCellsY(   ),   (  unsigned int )6 );
          
           // check that we actually get the windo that we expect
           for (  unsigned int i = 0; i < windowCopy.getSizeInCellsX(   ); ++i ) {
           for (  unsigned int j = 0; j < windowCopy.getSizeInCellsY(   ); ++j ) {
           ASSERT_EQ(  windowCopy.getCost(  i,   j ),   map.getCost(  i + 2,   j + 2 ) );
           // printf(  "%3d ",   windowCopy.getCost(  i,   j ) );
           }
           // printf(  "\n" );
           }
          }
          
          // test for updating costmaps with static data
     370  TEST(  costmap,   testFullyContainedStaticMapUpdate ) {
           nav2_costmap_2d::Costmap2D map(  5,   5,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_5_BY_5,   THRESHOLD );
          
           nav2_costmap_2d::Costmap2D static_map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           map.updateStaticMapWindow(  0,   0,   10,   10,   MAP_10_BY_10 );
          
           for (  unsigned int i = 0; i < map.getSizeInCellsX(   ); ++i ) {
           for (  unsigned int j = 0; j < map.getSizeInCellsY(   ); ++j ) {
           ASSERT_EQ(  map.getCost(  i,   j ),   static_map.getCost(  i,   j ) );
           }
           }
          }
          
     388  TEST(  costmap,   testOverlapStaticMapUpdate ) {
           nav2_costmap_2d::Costmap2D map(  5,   5,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_5_BY_5,   THRESHOLD );
          
           nav2_costmap_2d::Costmap2D static_map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           map.updateStaticMapWindow(  -10,   -10,   10,   10,   MAP_10_BY_10 );
          
           ASSERT_FLOAT_EQ(  map.getOriginX(   ),   -10 );
           ASSERT_FLOAT_EQ(  map.getOriginX(   ),   -10 );
           ASSERT_EQ(  map.getSizeInCellsX(   ),   (  unsigned int )15 );
           ASSERT_EQ(  map.getSizeInCellsY(   ),   (  unsigned int )15 );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           ASSERT_EQ(  map.getCost(  i,   j ),   static_map.getCost(  i,   j ) );
           }
           }
          
           std::vector<unsigned char> blank(  100 );
          
           // check to make sure that inflation on updates are being done correctly
           map.updateStaticMapWindow(  -10,   -10,   10,   10,   blank );
          
           for (  unsigned int i = 0; i < map.getSizeInCellsX(   ); ++i ) {
           for (  unsigned int j = 0; j < map.getSizeInCellsY(   ); ++j ) {
           ASSERT_EQ(  map.getCost(  i,   j ),   0 );
           }
           }
          
           std::vector<unsigned char> fully_contained(  25 );
           fully_contained[0] = 254;
           fully_contained[4] = 254;
           fully_contained[5] = 254;
           fully_contained[9] = 254;
          
           nav2_costmap_2d::Costmap2D small_static_map(  5,   5,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   fully_contained,   THRESHOLD );
          
           map.updateStaticMapWindow(  0,   0,   5,   5,   fully_contained );
          
           ASSERT_FLOAT_EQ(  map.getOriginX(   ),   -10 );
           ASSERT_FLOAT_EQ(  map.getOriginX(   ),   -10 );
           ASSERT_EQ(  map.getSizeInCellsX(   ),   (  unsigned int )15 );
           ASSERT_EQ(  map.getSizeInCellsY(   ),   (  unsigned int )15 );
           for (  unsigned int j = 0; j < 5; ++j ) {
           for (  unsigned int i = 0; i < 5; ++i ) {
           ASSERT_EQ(  map.getCost(  i + 10,   j + 10 ),   small_static_map.getCost(  i,   j ) );
           }
           }
          }
          
          /**
           * Test for ray tracing free space
           */
     446  TEST(  costmap,   testRaytracing ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // Add a point cloud,   should not affect the map
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  1 );
           cloud.points[0].x = 0;
           cloud.points[0].y = 0;
           cloud.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           int lethal_count = 0;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           lethal_count++;
           }
           }
           }
          
           // we expect just one obstacle to be added
           ASSERT_EQ(  lethal_count,   21 );
          }
          
     483  TEST(  costmap,   testAdjacentToObstacleCanStillMove ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,   2.1,   3.1,   4.1,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  1 );
           cloud.points[0].x = 0;
           cloud.points[0].y = 0;
           cloud.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  9,   9,   obsBuf,   obsBuf );
          
           EXPECT_EQ(  nav2_costmap_2d::LETHAL_OBSTACLE,   map.getCost(  0,   0 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   map.getCost(  1,   0 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   map.getCost(  2,   0 ) );
           EXPECT_TRUE(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > map.getCost(  3,   0 ) );
           EXPECT_TRUE(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > map.getCost(  2,   1 ) );
           EXPECT_EQ(  nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   map.getCost(  1,   1 ) );
          }
          
     511  TEST(  costmap,   testInflationShouldNotCreateUnknowns ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,   2.1,   3.1,   4.1,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  1 );
           cloud.points[0].x = 0;
           cloud.points[0].y = 0;
           cloud.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  9,   9,   obsBuf,   obsBuf );
          
           int unknown_count = 0;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::NO_INFORMATION ) {
           unknown_count++;
           }
           }
           }
           EXPECT_EQ(  0,   unknown_count );
          }
          
     543  unsigned int worldToIndex(  nav2_costmap_2d::Costmap2D & map,   double wx,   double wy )
          {
           unsigned int mx,   my;
           map.worldToMap(  wx,   wy,   mx,   my );
           return map.getIndex(  mx,   my );
          }
          
     550  void indexToWorld(  nav2_costmap_2d::Costmap2D & map,   unsigned int index,   double & wx,   double & wy )
          {
           unsigned int mx,   my;
           map.indexToCells(  index,   mx,   my );
           map.mapToWorld(  mx,   my,   wx,   wy );
          }
          
     557  TEST(  costmap,   testStaticMap ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           ASSERT_EQ(  map.getSizeInCellsX(   ),   (  unsigned int )10 );
           ASSERT_EQ(  map.getSizeInCellsY(   ),   (  unsigned int )10 );
          
           // Verify that obstacles correctly identified from the static map.
           std::vector<unsigned int> occupiedCells;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           occupiedCells.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  occupiedCells.size(   ),   (  unsigned int )20 );
          
           // Iterate over all id's and verify that they are present according to their
           for (  std::vector<unsigned int>::const_iterator it = occupiedCells.begin(   );
           it != occupiedCells.end(   ); ++it )
           {
           unsigned int ind = *it;
           unsigned int x,   y;
           map.indexToCells(  ind,   x,   y );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  x,   y ) ),   true );
           ASSERT_EQ(  MAP_10_BY_10[ind] >= 100,   true );
           ASSERT_EQ(  map.getCost(  x,   y ) >= 100,   true );
           }
          
           // Block of 200
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  7,   2 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  8,   2 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  9,   2 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  7,   3 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  8,   3 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  9,   3 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  7,   4 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  8,   4 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  9,   4 ) ),   true );
          
           // Block of 100
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  4,   3 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  4,   4 ) ),   true );
          
           // Block of 200
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  3,   7 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  4,   7 ) ),   true );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  5,   7 ) ),   true );
          
          
           // Verify Coordinate Transformations,   ROW MAJOR ORDER
           ASSERT_EQ(  worldToIndex(  map,   0.0,   0.0 ),   (  unsigned int )0 );
           ASSERT_EQ(  worldToIndex(  map,   0.0,   0.99 ),   (  unsigned int )0 );
           ASSERT_EQ(  worldToIndex(  map,   0.0,   1.0 ),   (  unsigned int )10 );
           ASSERT_EQ(  worldToIndex(  map,   1.0,   0.99 ),   (  unsigned int )1 );
           ASSERT_EQ(  worldToIndex(  map,   9.99,   9.99 ),   (  unsigned int )99 );
           ASSERT_EQ(  worldToIndex(  map,   8.2,   3.4 ),   (  unsigned int )38 );
          
           // Ensure we hit the middle of the cell for world co-ordinates
           double wx,   wy;
           indexToWorld(  map,   99,   wx,   wy );
           ASSERT_EQ(  wx,   9.5 );
           ASSERT_EQ(  wy,   9.5 );
          }
          
          
          /**
           * Verify that dynamic obstacles are added
           */
          
     631  TEST(  costmap,   testDynamicObstacles ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // Add a point cloud and verify its insertion. There should be only one new one
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  3 );
           cloud.points[0].x = 0;
           cloud.points[0].y = 0;
           cloud.points[1].x = 0;
           cloud.points[1].y = 0;
           cloud.points[2].x = 0;
           cloud.points[2].y = 0;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           std::vector<unsigned int> ids;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           // Should now have 1 insertion and no deletions
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )21 );
          
           // Repeating the call - we should see no insertions or deletions
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )21 );
          }
          
          /**
           * Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
           */
     678  TEST(  costmap,   testMultipleAdditions ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // A point cloud with one point that falls within an existing obstacle
           pcl::PointCloud<pcl::PointXYZ> cloud;
           cloud.points.resize(  1 );
           cloud.points[0].x = 7;
           cloud.points[0].y = 2;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           std::vector<unsigned int> ids;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )20 );
          }
          
          /**
           * Make sure we ignore points outside of our z threshold
           */
     716  TEST(  costmap,   testZThreshold ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // A point cloud with 2 points falling in a cell with a non-lethal cost
           pcl::PointCloud<pcl::PointXYZ> c0;
           c0.points.resize(  2 );
           c0.points[0].x = 0;
           c0.points[0].y = 5;
           c0.points[0].z = 0.4;
           c0.points[1].x = 1;
           c0.points[1].y = 5;
           c0.points[1].z = 1.2;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   c0,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           std::vector<unsigned int> ids;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )21 );
          }
          
          /**
           * Test inflation for both static and dynamic obstacles
           */
          
     758  TEST(  costmap,   testInflation ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // Verify that obstacles correctly identified
           std::vector<unsigned int> occupiedCells;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  i,   j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           occupiedCells.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           // There should be no duplicates
           std::set<unsigned int> setOfCells;
           for (  unsigned int i = 0; i < occupiedCells.size(   ); i++ ) {
           setOfCells.insert(  i );
           }
          
           ASSERT_EQ(  setOfCells.size(   ),   occupiedCells.size(   ) );
           ASSERT_EQ(  setOfCells.size(   ),   (  unsigned int )48 );
          
           // Iterate over all id's and verify they are obstacles
           for (  std::vector<unsigned int>::const_iterator it = occupiedCells.begin(   );
           it != occupiedCells.end(   ); ++it )
           {
           unsigned int ind = *it;
           unsigned int x,   y;
           map.indexToCells(  ind,   x,   y );
           ASSERT_EQ(  find(  occupiedCells,   map.getIndex(  x,   y ) ),   true );
           ASSERT_EQ(  
           map.getCost(  x,   y ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  x,   y ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,   true );
           }
          
           // Set an obstacle at the origin and observe insertions for it and its neighbors
           pcl::PointCloud<pcl::PointXYZ> c0;
           c0.points.resize(  1 );
           c0.points[0].x = 0;
           c0.points[0].y = 0;
           c0.points[0].z = 0.4;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   c0,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf,   empty;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   empty );
          
           occupiedCells.clear(   );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  i,   j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           occupiedCells.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           // It and its 2 neighbors makes 3 obstacles
           ASSERT_EQ(  occupiedCells.size(   ),   (  unsigned int )51 );
          
           // @todo Rewrite
           // Add an obstacle at <2,  0> which will inflate and refresh to of the other inflated cells
           pcl::PointCloud<pcl::PointXYZ> c1;
           c1.points.resize(  1 );
           c1.points[0].x = 2;
           c1.points[0].y = 0;
           c1.points[0].z = 0.0;
          
           geometry_msgs::Point p1;
           p1.x = 0.0;
           p1.y = 0.0;
           p1.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs1(  p1,   c1,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf1;
           obsBuf1.push_back(  obs1 );
          
           map.updateWorld(  0,   0,   obsBuf1,   empty );
          
           occupiedCells.clear(   );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  i,   j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           occupiedCells.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           // Now we expect insertions for it,   and 2 more neighbors,  
           // but not all 5. Free space will propagate from
           // the origin to the target,   clearing the point at <0,   0>,  
           // but not over-writing the inflation of the obstacle at <0,   1>
           ASSERT_EQ(  occupiedCells.size(   ),   (  unsigned int )54 );
          
          
           // Add an obstacle at <1,   9>. This will inflate obstacles around it
           pcl::PointCloud<pcl::PointXYZ> c2;
           c2.points.resize(  1 );
           c2.points[0].x = 1;
           c2.points[0].y = 9;
           c2.points[0].z = 0.0;
          
           geometry_msgs::Point p2;
           p2.x = 0.0;
           p2.y = 0.0;
           p2.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs2(  p2,   c2,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf2;
           obsBuf2.push_back(  obs2 );
          
           map.updateWorld(  0,   0,   obsBuf2,   empty );
          
           ASSERT_EQ(  map.getCost(  1,   9 ),   nav2_costmap_2d::LETHAL_OBSTACLE );
           ASSERT_EQ(  map.getCost(  0,   9 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
           ASSERT_EQ(  map.getCost(  2,   9 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
          
           // Add an obstacle and verify that it over-writes its inflated status
           pcl::PointCloud<pcl::PointXYZ> c3;
           c3.points.resize(  1 );
           c3.points[0].x = 0;
           c3.points[0].y = 9;
           c3.points[0].z = 0.0;
          
           geometry_msgs::Point p3;
           p3.x = 0.0;
           p3.y = 0.0;
           p3.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs3(  p3,   c3,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf3;
           obsBuf3.push_back(  obs3 );
          
           map.updateWorld(  0,   0,   obsBuf3,   empty );
          
           ASSERT_EQ(  map.getCost(  0,   9 ),   nav2_costmap_2d::LETHAL_OBSTACLE );
          }
          
          /**
           * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
           */
     913  TEST(  costmap,   testInflation2 ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           10.0,   MAX_Z,   10.0,   25,   MAP_10_BY_10,   THRESHOLD );
          
           // Creat a small L-Shape all at once
           pcl::PointCloud<pcl::PointXYZ> c0;
           c0.points.resize(  3 );
           c0.points[0].x = 1;
           c0.points[0].y = 1;
           c0.points[0].z = MAX_Z;
           c0.points[1].x = 1;
           c0.points[1].y = 2;
           c0.points[1].z = MAX_Z;
           c0.points[2].x = 2;
           c0.points[2].y = 2;
           c0.points[2].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   c0,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           ASSERT_EQ(  map.getCost(  3,   2 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
           ASSERT_EQ(  map.getCost(  3,   3 ),   nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
          }
          
          /**
           * Test inflation behavior,   starting with an empty map
           */
     949  TEST(  costmap,   testInflation3 ) {
           std::vector<unsigned char> mapData;
           for (  unsigned int i = 0; i < GRID_WIDTH; i++ ) {
           for (  unsigned int j = 0; j < GRID_HEIGHT; j++ ) {
           mapData.push_back(  0 );
           }
           }
          
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS * 2,   ROBOT_RADIUS * 3,  
           10.0,   MAX_Z,   10.0,   1,   mapData,   THRESHOLD );
          
           // There should be no occupied cells
           std::vector<unsigned int> ids;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  i,   j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )0 );
          
           // Add an obstacle at 5,  5
           pcl::PointCloud<pcl::PointXYZ> c0;
           c0.points.resize(  1 );
           c0.points[0].x = 5;
           c0.points[0].y = 5;
           c0.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.0;
           p.y = 0.0;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   c0,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) != nav2_costmap_2d::FREE_SPACE ) {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )29 );
          
           ids.clear(   );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
           map.getCost(  i,   j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )5 );
          
           // Update again - should see no change
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           ids.clear(   );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) != nav2_costmap_2d::FREE_SPACE ) {
           ids.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           ASSERT_EQ(  ids.size(   ),   (  unsigned int )29 );
          }
          
          /**
           * Test for ray tracing free space
           */
          
    1036  TEST(  costmap,   testRaytracing2 ) {
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           100.0,   MAX_Z,   100.0,   1,   MAP_10_BY_10,   THRESHOLD );
          
           // The sensor origin will be <0,  0>. So if we add an obstacle at 9,  9,   we would expect cells
           // <0,   0> thru <8,   8> to be traced through
           pcl::PointCloud<pcl::PointXYZ> c0;
           c0.points.resize(  1 );
           c0.points[0].x = 9.5;
           c0.points[0].y = 9.5;
           c0.points[0].z = MAX_Z;
          
           geometry_msgs::Point p;
           p.x = 0.5;
           p.y = 0.5;
           p.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs(  p,   c0,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf;
           obsBuf.push_back(  obs );
          
           std::vector<unsigned int> obstacles;
          
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           obstacles.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           unsigned int obs_before = obstacles.size(   );
          
           map.updateWorld(  0,   0,   obsBuf,   obsBuf );
          
           obstacles.clear(   );
           for (  unsigned int i = 0; i < 10; ++i ) {
           for (  unsigned int j = 0; j < 10; ++j ) {
           if (  map.getCost(  i,   j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           obstacles.push_back(  map.getIndex(  i,   j ) );
           }
           }
           }
          
           // Two obstacles should be removed from the map by raytracing
           ASSERT_EQ(  obstacles.size(   ),   obs_before - 2 );
          
          
           // many cells will have been switched to free space along the diagonal except
           // for those inflated in the update.. tests that inflation happens properly
           // after raytracing
           unsigned char test[10] = {0,   0,   0,   253,   253,   0,   0,   253,   253,   254};
           for (  unsigned int i = 0; i < 10; i++ ) {
           ASSERT_EQ(  map.getCost(  i,   i ),   test[i] );
           }
          }
          
          /**
           * Within a certian radius of the robot,   the cost map most propagate obstacles. This
           * is to avoid a case where a hit on a far obstacle clears inscribed radius around a
           * near one.
           */
          
    1100  TEST(  costmap,   testTrickyPropagation ) {
           const unsigned char MAP_HALL_CHAR[10 * 10] = {
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           254,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   254,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   254,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   254,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           };
           std::vector<unsigned char> MAP_HALL;
           for (  int i = 0; i < 10 * 10; i++ ) {
           MAP_HALL.push_back(  MAP_HALL_CHAR[i] );
           }
          
           nav2_costmap_2d::Costmap2D map(  GRID_WIDTH,   GRID_HEIGHT,   RESOLUTION,   0.0,   0.0,  
           ROBOT_RADIUS,   ROBOT_RADIUS,   ROBOT_RADIUS,  
           100.0,   MAX_Z,   100.0,   1,   MAP_HALL,   THRESHOLD );
          
          
           // Add a dynamic obstacle
           pcl::PointCloud<pcl::PointXYZ> c2;
           c2.points.resize(  3 );
           // Dynamic obstacle that raytaces.
           c2.points[0].x = 7.0;
           c2.points[0].y = 8.0;
           c2.points[0].z = 1.0;
           // Dynamic obstacle that should not be raytraced the
           // first update,   but should on the second.
           c2.points[1].x = 3.0;
           c2.points[1].y = 4.0;
           c2.points[1].z = 1.0;
           // Dynamic obstacle that should not be erased.
           c2.points[2].x = 6.0;
           c2.points[2].y = 3.0;
           c2.points[2].z = 1.0;
          
           geometry_msgs::Point p2;
           p2.x = 0.5;
           p2.y = 0.5;
           p2.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs2(  p2,   c2,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf2;
           obsBuf2.push_back(  obs2 );
          
           map.updateWorld(  0,   0,   obsBuf2,   obsBuf2 );
          
           const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = {
           253,   254,   253,   0,   0,   0,   0,   0,   0,   0,  
           0,   253,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   253,   0,   0,   0,   0,   0,  
           0,   0,   0,   253,   254,   253,   0,   0,   0,   0,  
           0,   0,   0,   0,   253,   0,   0,   253,   0,   0,  
           0,   0,   0,   253,   0,   0,   253,   254,   253,   0,  
           0,   0,   253,   254,   253,   0,   0,   253,   253,   0,  
           0,   0,   0,   253,   0,   0,   0,   253,   254,   253,  
           0,   0,   0,   0,   0,   0,   0,   0,   253,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           };
          
           for (  int i = 0; i < 10 * 10; i++ ) {
           ASSERT_EQ(  map.getCost(  i / 10,   i % 10 ),   MAP_HALL_CHAR_TEST[i] );
           }
          
           pcl::PointCloud<pcl::PointXYZ> c;
           c.points.resize(  1 );
           // Dynamic obstacle that raytaces the one at (  3.0,   4.0 ).
           c.points[0].x = 4.0;
           c.points[0].y = 5.0;
           c.points[0].z = 1.0;
          
           geometry_msgs::Point p3;
           p3.x = 0.5;
           p3.y = 0.5;
           p3.z = MAX_Z;
          
           nav2_costmap_2d::Observation obs3(  p3,   c,   100.0,   100.0 );
           std::vector<nav2_costmap_2d::Observation> obsBuf3;
           obsBuf3.push_back(  obs3 );
          
           map.updateWorld(  0,   0,   obsBuf3,   obsBuf3 );
          
           const unsigned char MAP_HALL_CHAR_TEST2[10 * 10] = {
           253,   254,   253,   0,   0,   0,   0,   0,   0,   0,  
           0,   253,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           0,   0,   0,   0,   0,   253,   0,   0,   0,   0,  
           0,   0,   0,   0,   253,   254,   253,   253,   0,   0,  
           0,   0,   0,   253,   0,   253,   253,   254,   253,   0,  
           0,   0,   253,   254,   253,   0,   0,   253,   253,   0,  
           0,   0,   0,   253,   0,   0,   0,   253,   254,   253,  
           0,   0,   0,   0,   0,   0,   0,   0,   253,   0,  
           0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  
           };
          
           for (  int i = 0; i < 10 * 10; i++ ) {
           ASSERT_EQ(  map.getCost(  i / 10,   i % 10 ),   MAP_HALL_CHAR_TEST2[i] );
           }
          }
          
          
    1205  int main(  int argc,   char ** argv )
          {
           for (  unsigned int i = 0; i < GRID_WIDTH * GRID_HEIGHT; i++ ) {
           EMPTY_10_BY_10.push_back(  0 );
           MAP_10_BY_10.push_back(  MAP_10_BY_10_CHAR[i] );
           }
          
           for (  unsigned int i = 0; i < 5 * 5; i++ ) {
           MAP_5_BY_5.push_back(  MAP_10_BY_10_CHAR[i] );
           }
          
           for (  unsigned int i = 0; i < 100 * 100; i++ ) {
           EMPTY_100_BY_100.push_back(  0 );
           }
          
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp

       1  /*********************************************************************
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2022 Samsung Research Russia
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of Willow Garage,   Inc. nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Alexey Merzlyakov
          *********************************************************************/
          #include <nav2_costmap_2d/costmap_2d.hpp>
          #include <gtest/gtest.h>
          
      39  class CostmapAction
          {
          public:
      42   explicit CostmapAction(  
           unsigned char * costmap,   unsigned int size,   unsigned char mark_val = 128 )
           : costmap_(  costmap ),   size_(  size ),   mark_val_(  mark_val )
           {
           }
          
      48   inline void operator(   )(  unsigned int off )
           {
           ASSERT_TRUE(  off < size_ );
           costmap_[off] = mark_val_;
           }
          
      54   inline unsigned int get(  unsigned int off )
           {
           return costmap_[off];
           }
          
          private:
           unsigned char * costmap_;
           unsigned int size_;
           unsigned char mark_val_;
          };
          
      65  class CostmapTest : public nav2_costmap_2d::Costmap2D
          {
          public:
      68   CostmapTest(  
           unsigned int size_x,   unsigned int size_y,   double resolution,  
           double origin_x,   double origin_y,   unsigned char default_val = 0 )
           : nav2_costmap_2d::Costmap2D(  size_x,   size_y,   resolution,   origin_x,   origin_y,   default_val )
           {
           }
          
      75   unsigned char * getCostmap(   )
           {
           return costmap_;
           }
          
      80   unsigned int getSize(   )
           {
           return size_x_ * size_y_;
           }
          
      85   void raytraceLine(  
           CostmapAction ca,   unsigned int x0,   unsigned int y0,   unsigned int x1,  
           unsigned int y1,  
           unsigned int max_length = UINT_MAX,   unsigned int min_length = 0 )
           {
           nav2_costmap_2d::Costmap2D::raytraceLine(  ca,   x0,   y0,   x1,   y1,   max_length,   min_length );
           }
          };
          
      94  TEST(  costmap_2d,   bresenham2DBoundariesCheck )
          {
           const unsigned int sz_x = 60;
           const unsigned int sz_y = 60;
           const unsigned int max_length = 60;
           const unsigned int min_length = 6;
           CostmapTest ct(  sz_x,   sz_y,   0.1,   0.0,   0.0 );
           CostmapAction ca(  ct.getCostmap(   ),   ct.getSize(   ) );
          
           // Initial point - some assymetrically standing point in order to cover most corner cases
           const unsigned int x0 = 2;
           const unsigned int y0 = 4;
           // (  x1,   y1 ) point will move
           unsigned int x1,   y1;
          
           // Running on (  x,   0 ) edge
           y1 = 0;
           for (  x1 = 0; x1 < sz_x; x1++ ) {
           ct.raytraceLine(  ca,   x0,   y0,   x1,   y1,   max_length,   min_length );
           }
          
           // Running on (  x,   sz_y ) edge
           y1 = sz_y - 1;
           for (  x1 = 0; x1 < sz_x; x1++ ) {
           ct.raytraceLine(  ca,   x0,   y0,   x1,   y1,   max_length,   min_length );
           }
          
           // Running on (  0,   y ) edge
           x1 = 0;
           for (  y1 = 0; y1 < sz_y; y1++ ) {
           ct.raytraceLine(  ca,   x0,   y0,   x1,   y1,   max_length,   min_length );
           }
          
           // Running on (  sz_x,   y ) edge
           x1 = sz_x - 1;
           for (  y1 = 0; y1 < sz_y; y1++ ) {
           ct.raytraceLine(  ca,   x0,   y0,   x1,   y1,   max_length,   min_length );
           }
          }
          
     134  TEST(  costmap_2d,   bresenham2DSamePoint )
          {
           const unsigned int sz_x = 60;
           const unsigned int sz_y = 60;
           const unsigned int max_length = 60;
           const unsigned int min_length = 0;
           CostmapTest ct(  sz_x,   sz_y,   0.1,   0.0,   0.0 );
           CostmapAction ca(  ct.getCostmap(   ),   ct.getSize(   ) );
          
           // Initial point
           const double x0 = 2;
           const double y0 = 4;
          
           unsigned int offset = y0 * sz_x + x0;
           unsigned char val_before = ca.get(  offset );
           // Same point to check
           ct.raytraceLine(  ca,   x0,   y0,   x0,   y0,   max_length,   min_length );
           unsigned char val_after = ca.get(  offset );
           ASSERT_FALSE(  val_before == val_after );
          }
          
     155  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_costmap_2d/test/testing_helper.hpp

       1  // Copyright (  c ) Willow Garage
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TESTING_HELPER_HPP_
          #define TESTING_HELPER_HPP_
          
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/static_layer.hpp"
          #include "nav2_costmap_2d/range_sensor_layer.hpp"
          #include "nav2_costmap_2d/obstacle_layer.hpp"
          #include "nav2_costmap_2d/inflation_layer.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
      31  const double MAX_Z(  1.0 );
          
      33  char printableCost(  unsigned char cost )
          {
           switch (  cost ) {
           case nav2_costmap_2d::NO_INFORMATION: return '?';
           case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L';
           case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
           case nav2_costmap_2d::FREE_SPACE: return '.';
           default: return '0' + (  unsigned char ) (  10 * cost / 255 );
           }
          }
          
      44  void printMap(  nav2_costmap_2d::Costmap2D & costmap )
          {
           printf(  "map:\n" );
           for (  unsigned int i = 0; i < costmap.getSizeInCellsY(   ); i++ ) {
           for (  unsigned int j = 0; j < costmap.getSizeInCellsX(   ); j++ ) {
           printf(  "%4d",   static_cast<int>(  costmap.getCost(  j,   i ) ) );
           }
           printf(  "\n\n" );
           }
          }
          
      55  unsigned int countValues(  
           nav2_costmap_2d::Costmap2D & costmap,  
           unsigned char value,   bool equal = true )
          {
           unsigned int count = 0;
           for (  unsigned int i = 0; i < costmap.getSizeInCellsY(   ); i++ ) {
           for (  unsigned int j = 0; j < costmap.getSizeInCellsX(   ); j++ ) {
           unsigned char c = costmap.getCost(  j,   i );
           if (  (  equal && c == value ) || (  !equal && c != value ) ) {
           count += 1;
           }
           }
           }
           return count;
          }
          
      71  void addStaticLayer(  
           nav2_costmap_2d::LayeredCostmap & layers,  
           tf2_ros::Buffer & tf,   nav2_util::LifecycleNode::SharedPtr node,  
           std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer,  
           rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
          {
           slayer = std::make_shared<nav2_costmap_2d::StaticLayer>(   );
           layers.addPlugin(  std::shared_ptr<nav2_costmap_2d::Layer>(  slayer ) );
           slayer->initialize(  &layers,   "static",   &tf,   node,   callback_group );
          }
          
      82  void addObstacleLayer(  
           nav2_costmap_2d::LayeredCostmap & layers,  
           tf2_ros::Buffer & tf,   nav2_util::LifecycleNode::SharedPtr node,  
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer,  
           rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
          {
           olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>(   );
           olayer->initialize(  &layers,   "obstacles",   &tf,   node,   callback_group );
           layers.addPlugin(  std::shared_ptr<nav2_costmap_2d::Layer>(  olayer ) );
          }
          
      93  void addRangeLayer(  
           nav2_costmap_2d::LayeredCostmap & layers,  
           tf2_ros::Buffer & tf,   nav2_util::LifecycleNode::SharedPtr node,  
           std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> & rlayer,  
           rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
          {
           rlayer = std::make_shared<nav2_costmap_2d::RangeSensorLayer>(   );
           rlayer->initialize(  &layers,   "range",   &tf,   node,   callback_group );
           layers.addPlugin(  std::shared_ptr<nav2_costmap_2d::Layer>(  rlayer ) );
          }
          
     104  void addObservation(  
           std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer,   double x,   double y,   double z = 0.0,  
           double ox = 0.0,   double oy = 0.0,   double oz = MAX_Z,   bool marking = true,   bool clearing = true,  
           double raytrace_max_range = 100.0,  
           double raytrace_min_range = 0.0,  
           double obstacle_max_range = 100.0,  
           double obstacle_min_range = 0.0 )
          {
           sensor_msgs::msg::PointCloud2 cloud;
           sensor_msgs::PointCloud2Modifier modifier(  cloud );
           modifier.setPointCloud2FieldsByString(  1,   "xyz" );
           modifier.resize(  1 );
           sensor_msgs::PointCloud2Iterator<float> iter_x(  cloud,   "x" );
           sensor_msgs::PointCloud2Iterator<float> iter_y(  cloud,   "y" );
           sensor_msgs::PointCloud2Iterator<float> iter_z(  cloud,   "z" );
           *iter_x = x;
           *iter_y = y;
           *iter_z = z;
          
           geometry_msgs::msg::Point p;
           p.x = ox;
           p.y = oy;
           p.z = oz;
          
           nav2_costmap_2d::Observation obs(  p,   cloud,   obstacle_max_range,   obstacle_min_range,  
           raytrace_max_range,   raytrace_min_range );
           olayer->addStaticObservation(  obs,   marking,   clearing );
          }
          
     133  void addInflationLayer(  
           nav2_costmap_2d::LayeredCostmap & layers,  
           tf2_ros::Buffer & tf,   nav2_util::LifecycleNode::SharedPtr node,  
           std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,  
           rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
          {
           ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>(   );
           ilayer->initialize(  &layers,   "inflation",   &tf,   node,   callback_group );
           std::shared_ptr<nav2_costmap_2d::Layer> ipointer(  ilayer );
           layers.addPlugin(  ipointer );
          }
          
          
          #endif // TESTING_HELPER_HPP_

navigation2/nav2_costmap_2d/test/unit/array_parser_test.cpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_costmap_2d/array_parser.hpp"
          
      36  TEST(  array_parser,   basic_operation )
          {
           std::string error;
           std::vector<std::vector<float>> vvf;
           vvf = nav2_costmap_2d::parseVVF(  "[[1,   2.2],   [.3,   -4e4]]",   error );
           EXPECT_EQ(  2u,   vvf.size(   )  );
           EXPECT_EQ(  2u,   vvf[0].size(   )  );
           EXPECT_EQ(  2u,   vvf[1].size(   )  );
           EXPECT_EQ(  1.0f,   vvf[0][0] );
           EXPECT_EQ(  2.2f,   vvf[0][1] );
           EXPECT_EQ(  0.3f,   vvf[1][0] );
           EXPECT_EQ(  -40000.0f,   vvf[1][1] );
           EXPECT_EQ(  "",   error );
          }
          
      51  TEST(  array_parser,   missing_open )
          {
           std::string error;
           std::vector<std::vector<float>> vvf;
           vvf = nav2_costmap_2d::parseVVF(  "[1,   2.2],   [.3,   -4e4]]",   error );
           EXPECT_NE(  error,   "" );
          }
          
      59  TEST(  array_parser,   missing_close )
          {
           std::string error;
           std::vector<std::vector<float>> vvf;
           vvf = nav2_costmap_2d::parseVVF(  "[[1,   2.2],   [.3,   -4e4]",   error );
           EXPECT_NE(  error,   "" );
          }
          
      67  TEST(  array_parser,   wrong_depth )
          {
           std::string error;
           std::vector<std::vector<float>> vvf;
           vvf = nav2_costmap_2d::parseVVF(  "[1,   2.2],   [.3,   -4e4]",   error );
           EXPECT_NE(  error,   "" );
          }
          
      75  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_costmap_2d/test/unit/copy_window_test.cpp

       1  // Copyright (  c ) 2021 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          
      20  class RclCppFixture
          {
          public:
      23   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      24   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      28  TEST(  CopyWindow,   copyValidWindow )
          {
           nav2_costmap_2d::Costmap2D src(  10,   10,   0.1,   0.0,   0.0 );
           nav2_costmap_2d::Costmap2D dst(  5,   5,   0.2,   100.0,   100.0 );
           // Adding 2 marked cells to source costmap
           src.setCost(  2,   2,   100 );
           src.setCost(  5,   5,   200 );
          
           ASSERT_TRUE(  dst.copyWindow(  src,   2,   2,   6,   6,   0,   0 ) );
           // Check that both marked cells were copied to destination costmap
           ASSERT_EQ(  dst.getCost(  0,   0 ),   100 );
           ASSERT_EQ(  dst.getCost(  3,   3 ),   200 );
          }
          
      42  TEST(  CopyWindow,   copyInvalidWindow )
          {
           nav2_costmap_2d::Costmap2D src(  10,   10,   0.1,   0.0,   0.0 );
           nav2_costmap_2d::Costmap2D dst(  5,   5,   0.2,   100.0,   100.0 );
          
           // Case1: incorrect source bounds
           ASSERT_FALSE(  dst.copyWindow(  src,   9,   9,   11,   11,   0,   0 ) );
           // Case2: incorrect destination bounds
           ASSERT_FALSE(  dst.copyWindow(  src,   0,   0,   1,   1,   5,   5 ) );
           ASSERT_FALSE(  dst.copyWindow(  src,   0,   0,   6,   6,   0,   0 ) );
          }

navigation2/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          
          #include <vector>
          #include <cmath>
          #include <limits>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/occ_grid_values.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          
      28  class RclCppFixture
          {
          public:
      31   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      32   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      36  static constexpr double EPSILON = std::numeric_limits<float>::epsilon(   );
      37  static constexpr double RESOLUTION = 0.05;
      38  static constexpr double ORIGIN_X = 0.1;
      39  static constexpr double ORIGIN_Y = 0.2;
          
      41  class TestNode : public ::testing::Test
          {
          public:
           TestNode(   ) {}
          
           ~TestNode(   )
           {
           occ_grid_.reset(   );
           costmap_.reset(   );
           }
          
          protected:
           void createMaps(   );
           void verifyCostmap(   );
          
          private:
           std::shared_ptr<nav_msgs::msg::OccupancyGrid> occ_grid_;
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
          };
          
      61  void TestNode::createMaps(   )
          {
           // Create occ_grid_ map
           occ_grid_ = std::make_shared<nav_msgs::msg::OccupancyGrid>(   );
          
           const unsigned int width = 4;
           const unsigned int height = 3;
          
           occ_grid_->info.resolution = RESOLUTION;
           occ_grid_->info.width = width;
           occ_grid_->info.height = height;
           occ_grid_->info.origin.position.x = ORIGIN_X;
           occ_grid_->info.origin.position.y = ORIGIN_Y;
           occ_grid_->info.origin.position.z = 0.0;
           occ_grid_->info.origin.orientation.x = 0.0;
           occ_grid_->info.origin.orientation.y = 0.0;
           occ_grid_->info.origin.orientation.z = 0.0;
           occ_grid_->info.origin.orientation.w = 1.0;
           occ_grid_->data.resize(  width * height );
          
           int8_t data;
           for (  unsigned int i = 0; i < width * height; i++ ) {
           data = i * 10;
           if (  data <= nav2_util::OCC_GRID_OCCUPIED ) {
           occ_grid_->data[i] = data;
           } else {
           occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN;
           }
           }
          
           // Create costmap_ (  convert OccupancyGrid -> to Costmap2D )
           costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(  *occ_grid_ );
          }
          
      95  void TestNode::verifyCostmap(   )
          {
           // Verify Costmap2D info
           EXPECT_NEAR(  costmap_->getResolution(   ),   RESOLUTION,   EPSILON );
           EXPECT_NEAR(  costmap_->getOriginX(   ),   ORIGIN_X,   EPSILON );
           EXPECT_NEAR(  costmap_->getOriginY(   ),   ORIGIN_Y,   EPSILON );
          
           // Verify Costmap2D data
           unsigned int it;
           unsigned char data,   data_ref;
           for (  it = 0; it < (  costmap_->getSizeInCellsX(   ) * costmap_->getSizeInCellsY(   ) - 1 ); it++ ) {
           data = costmap_->getCharMap(   )[it];
           if (  it != costmap_->getSizeInCellsX(   ) * costmap_->getSizeInCellsY(   ) - 1 ) {
           data_ref = std::round(  
           static_cast<double>(  nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE ) * it /
           10 );
           } else {
           data_ref = nav2_costmap_2d::NO_INFORMATION;
           }
           EXPECT_EQ(  data,   data_ref );
           }
          }
          
     118  TEST_F(  TestNode,   convertOccGridToCostmap )
          {
           createMaps(   );
           verifyCostmap(   );
          }

navigation2/nav2_costmap_2d/test/unit/declare_parameter_test.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/layer.hpp"
          
      23  class RclCppFixture
          {
          public:
      26   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      27   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      31  class LayerWrapper : public nav2_costmap_2d::Layer
          {
      33   void reset(   ) {}
      34   void updateBounds(  double,   double,   double,   double *,   double *,   double *,   double * ) {}
      35   void updateCosts(  nav2_costmap_2d::Costmap2D &,   int,   int,   int,   int ) {}
      36   bool isClearable(   ) {return false;}
          };
          
      39  TEST(  DeclareParameter,   useValidParameter )
          {
           LayerWrapper layer;
           nav2_util::LifecycleNode::SharedPtr node =
           std::make_shared<nav2_util::LifecycleNode>(  "test_node" );
           tf2_ros::Buffer tf(  node->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           layer.initialize(  &layers,   "test_layer",   &tf,   node,   nullptr );
          
           layer.declareParameter(  "test1",   rclcpp::ParameterValue(  "test_val1" ) );
           try {
           std::string val = node->get_parameter(  "test_layer.test1" ).as_string(   );
           EXPECT_EQ(  val,   "test_val1" );
           } catch (  rclcpp::exceptions::ParameterNotDeclaredException & ex ) {
           FAIL(   ) << "test_layer.test1 parameter is not set";
           }
          }
          
      58  TEST(  DeclareParameter,   useInvalidParameter )
          {
           LayerWrapper layer;
           nav2_util::LifecycleNode::SharedPtr node =
           std::make_shared<nav2_util::LifecycleNode>(  "test_node" );
           tf2_ros::Buffer tf(  node->get_clock(   ) );
           nav2_costmap_2d::LayeredCostmap layers(  "frame",   false,   false );
          
           layer.initialize(  &layers,   "test_layer",   &tf,   node,   nullptr );
          
           layer.declareParameter(  "test2",   rclcpp::PARAMETER_STRING );
           try {
           std::string val = node->get_parameter(  "test_layer.test2" ).as_string(   );
           FAIL(   ) << "Incorrectly handling test_layer.test2 parameter which was not set";
           } catch (  rclcpp::exceptions::ParameterUninitializedException & ex ) {
           SUCCEED(   );
           }
          }

navigation2/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp

       1  // Copyright (  c ) 2020 Shivang Patel
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <vector>
          #include <memory>
          
          #include "gtest/gtest.h"
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "nav2_costmap_2d/footprint.hpp"
          
      23  TEST(  collision_footprint,   test_basic )
          {
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
           std::make_shared<nav2_costmap_2d::Costmap2D>(  100,   100,   0.1,   0,   0,   0 );
          
           geometry_msgs::msg::Point p1;
           p1.x = -0.5;
           p1.y = 0.0;
           geometry_msgs::msg::Point p2;
           p2.x = 0.0;
           p2.y = 0.5;
           geometry_msgs::msg::Point p3;
           p3.x = 0.5;
           p3.y = 0.0;
           geometry_msgs::msg::Point p4;
           p4.x = 0.0;
           p4.y = -0.5;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
           collision_checker(  costmap_ );
          
           auto value = collision_checker.footprintCostAtPose(  5.0,   5.0,   0.0,   footprint );
          
           EXPECT_NEAR(  value,   0.0,   0.001 );
          }
          
      51  TEST(  collision_footprint,   test_point_cost )
          {
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
           std::make_shared<nav2_costmap_2d::Costmap2D>(  100,   100,   0.1,   0,   0,   0 );
          
           nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
           collision_checker(  costmap_ );
          
           auto value = collision_checker.pointCost(  50,   50 );
          
           EXPECT_NEAR(  value,   0.0,   0.001 );
          }
          
      64  TEST(  collision_footprint,   test_world_to_map )
          {
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
           std::make_shared<nav2_costmap_2d::Costmap2D>(  100,   100,   0.1,   0,   0,   0 );
          
           nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
           collision_checker(  costmap_ );
          
           unsigned int x,   y;
          
           collision_checker.worldToMap(  1.0,   1.0,   x,   y );
          
           auto value = collision_checker.pointCost(  x,   y );
          
           EXPECT_NEAR(  value,   0.0,   0.001 );
          
           costmap_->setCost(  50,   50,   200 );
           collision_checker.worldToMap(  5.0,   5.0,   x,   y );
          
           EXPECT_NEAR(  collision_checker.pointCost(  x,   y ),   200.0,   0.001 );
          }
          
      86  TEST(  collision_footprint,   test_footprint_at_pose_with_movement )
          {
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
           std::make_shared<nav2_costmap_2d::Costmap2D>(  100,   100,   0.1,   0,   0,   254 );
          
           for (  unsigned int i = 40; i <= 60; ++i ) {
           for (  unsigned int j = 40; j <= 60; ++j ) {
           costmap_->setCost(  i,   j,   0 );
           }
           }
          
           geometry_msgs::msg::Point p1;
           p1.x = -1.0;
           p1.y = 1.0;
           geometry_msgs::msg::Point p2;
           p2.x = 1.0;
           p2.y = 1.0;
           geometry_msgs::msg::Point p3;
           p3.x = 1.0;
           p3.y = -1.0;
           geometry_msgs::msg::Point p4;
           p4.x = -1.0;
           p4.y = -1.0;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
           collision_checker(  costmap_ );
          
           auto value = collision_checker.footprintCostAtPose(  5.0,   5.0,   0.0,   footprint );
           EXPECT_NEAR(  value,   0.0,   0.001 );
          
           auto up_value = collision_checker.footprintCostAtPose(  5.0,   4.9,   0.0,   footprint );
           EXPECT_NEAR(  up_value,   254.0,   0.001 );
          
           auto down_value = collision_checker.footprintCostAtPose(  5.0,   5.2,   0.0,   footprint );
           EXPECT_NEAR(  down_value,   254.0,   0.001 );
          }
          
     125  TEST(  collision_footprint,   test_point_and_line_cost )
          {
           std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
           std::make_shared<nav2_costmap_2d::Costmap2D>(  100,   100,   0.10000,   0,   0.0,   0.0 );
          
           costmap_->setCost(  62,   50,   254 );
           costmap_->setCost(  39,   60,   254 );
          
           geometry_msgs::msg::Point p1;
           p1.x = -1.0;
           p1.y = 1.0;
           geometry_msgs::msg::Point p2;
           p2.x = 1.0;
           p2.y = 1.0;
           geometry_msgs::msg::Point p3;
           p3.x = 1.0;
           p3.y = -1.0;
           geometry_msgs::msg::Point p4;
           p4.x = -1.0;
           p4.y = -1.0;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
           collision_checker(  costmap_ );
          
           auto value = collision_checker.footprintCostAtPose(  5.0,   5.0,   0.0,   footprint );
           EXPECT_NEAR(  value,   0.0,   0.001 );
          
           auto left_value = collision_checker.footprintCostAtPose(  4.9,   5.0,   0.0,   footprint );
           EXPECT_NEAR(  left_value,   254.0,   0.001 );
          
           auto right_value = collision_checker.footprintCostAtPose(  5.2,   5.0,   0.0,   footprint );
           EXPECT_NEAR(  right_value,   254.0,   0.001 );
          }
          
     161  TEST(  collision_footprint,   not_enough_points )
          {
           geometry_msgs::msg::Point p1;
           p1.x = 2.0;
           p1.y = 2.0;
          
           geometry_msgs::msg::Point p2;
           p2.x = -2.0;
           p2.y = -2.0;
          
           std::vector<geometry_msgs::msg::Point> footprint = {p1,   p2};
           double min_dist = 0.0;
           double max_dist = 0.0;
          
           nav2_costmap_2d::calculateMinAndMaxDistances(  footprint,   min_dist,   max_dist );
           EXPECT_EQ(  min_dist,   std::numeric_limits<double>::max(   ) );
           EXPECT_EQ(  max_dist,   0.0f );
          }
          
     180  TEST(  collision_footprint,   to_point_32 ) {
           geometry_msgs::msg::Point p;
           p.x = 123.0;
           p.y = 456.0;
           p.z = 789.0;
          
           geometry_msgs::msg::Point32 p32;
           p32 = nav2_costmap_2d::toPoint32(  p );
           EXPECT_NEAR(  p.x,   p32.x,   1e-5 );
           EXPECT_NEAR(  p.y,   p32.y,   1e-5 );
           EXPECT_NEAR(  p.z,   p32.z,   1e-5 );
          }
          
     193  TEST(  collision_footprint,   to_polygon ) {
           geometry_msgs::msg::Point p1;
           p1.x = 1.2;
           p1.y = 3.4;
           p1.z = 5.1;
          
           geometry_msgs::msg::Point p2;
           p2.x = -5.6;
           p2.y = -7.8;
           p2.z = -9.1;
           std::vector<geometry_msgs::msg::Point> pts = {p1,   p2};
          
           geometry_msgs::msg::Polygon poly;
           poly = nav2_costmap_2d::toPolygon(  pts );
          
           EXPECT_EQ(  2u,   sizeof(  poly.points ) / sizeof(  poly.points[0] ) );
           EXPECT_NEAR(  poly.points[0].x,   p1.x,   1e-5 );
           EXPECT_NEAR(  poly.points[0].y,   p1.y,   1e-5 );
           EXPECT_NEAR(  poly.points[0].z,   p1.z,   1e-5 );
           EXPECT_NEAR(  poly.points[1].x,   p2.x,   1e-5 );
           EXPECT_NEAR(  poly.points[1].y,   p2.y,   1e-5 );
           EXPECT_NEAR(  poly.points[1].z,   p2.z,   1e-5 );
          }
          
     217  TEST(  collision_footprint,   make_footprint_from_string ) {
           std::vector<geometry_msgs::msg::Point> footprint;
           bool result = nav2_costmap_2d::makeFootprintFromString(  
           "[[1,   2.2],   [.3,   -4e4],   [-.3,   -4e4],   [-1,   2.2]]",   footprint );
           EXPECT_EQ(  result,   true );
           EXPECT_EQ(  4u,   footprint.size(   ) );
           EXPECT_NEAR(  footprint[0].x,   1.0,   1e-5 );
           EXPECT_NEAR(  footprint[0].y,   2.2,   1e-5 );
           EXPECT_NEAR(  footprint[1].x,   0.3,   1e-5 );
           EXPECT_NEAR(  footprint[1].y,   -4e4,   1e-5 );
           EXPECT_NEAR(  footprint[2].x,   -0.3,   1e-5 );
           EXPECT_NEAR(  footprint[2].y,   -4e4,   1e-5 );
           EXPECT_NEAR(  footprint[3].x,   -1.0,   1e-5 );
           EXPECT_NEAR(  footprint[3].y,   2.2,   1e-5 );
          }
          
     233  TEST(  collision_footprint,   make_footprint_from_string_parse_error ) {
           std::vector<geometry_msgs::msg::Point> footprint;
           bool result = nav2_costmap_2d::makeFootprintFromString(  
           "[[bad_string",   footprint );
           EXPECT_EQ(  result,   false );
          }
          
     240  TEST(  collision_footprint,   make_footprint_from_string_two_points_error ) {
           std::vector<geometry_msgs::msg::Point> footprint;
           bool result = nav2_costmap_2d::makeFootprintFromString(  
           "[[1,   2.2],   [.3,   -4e4]",   footprint );
           EXPECT_EQ(  result,   false );
          }
          
     247  TEST(  collision_footprint,   make_footprint_from_string_not_pairs ) {
           std::vector<geometry_msgs::msg::Point> footprint;
           bool result = nav2_costmap_2d::makeFootprintFromString(  
           "[[1,   2.2],   [.3,   -4e4],   [-.3,   -4e4],   [-1,   2.2,   5.6]]",   footprint );
           EXPECT_EQ(  result,   false );
          }

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
          #define COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
          
          #include <cmath>
          #include <vector>
          #include <limits>
          #include <memory>
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "costmap_queue/map_based_queue.hpp"
          
          namespace costmap_queue
          {
          /**
           * @class CellData
           * @brief Storage for cell information used during queue expansion
           */
      51  class CellData
          {
          public:
           /**
           * @brief Real Constructor
           * @param d The distance to the nearest obstacle
           * @param i The index of the cell in the costmap. Redundant with the following two parameters.
           * @param x The x coordinate of the cell in the cost map
           * @param y The y coordinate of the cell in the cost map
           * @param sx The x coordinate of the closest source cell in the costmap
           * @param sy The y coordinate of the closest source cell in the costmap
           */
      63   CellData(  
           const double d,   const unsigned int i,   const unsigned int x,   const unsigned int y,  
           const unsigned int sx,   const unsigned int sy )
           : distance_(  d ),   index_(  i ),   x_(  x ),   y_(  y ),   src_x_(  sx ),   src_y_(  sy )
           {
           }
          
           /**
           * @brief Default Constructor - Should be used sparingly
           */
           CellData(   )
           : distance_(  std::numeric_limits<double>::max(   ) ),   index_(  0 ),   x_(  0 ),   y_(  0 ),   src_x_(  0 ),   src_y_(  0 )
           {
           }
          
           static unsigned absolute_difference(  const unsigned x,   const unsigned y )
           {
           return (  x > y ) ? (  x - y ) : (  y - x );
           }
          
           double distance_;
           unsigned int index_;
           unsigned int x_,   y_;
           unsigned int src_x_,   src_y_;
          };
          
          /**
           * @class CostmaQueue
           * @brief A tool for finding the cells closest to some set of originating cells.
           *
           * A common operation with costmaps is to define a set of cells in the costmap,   and then
           * perform some operation on all the other cells based on which cell in the original set
           * the other cells are closest to. This operation is done in the inflation layer to figure out
           * how far each cell is from an obstacle,   and is also used in a number of Trajectory cost functions.
           *
           * It is implemented with a queue. The standard operation is to enqueueCell the original set,   and then
           * retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell
           * returns an object that contains the coordinates of this cell and the origin cell,   as well as
           * the distance between them. By default,   the Euclidean distance is used for ordering,   but passing in
           * manhattan=true to the constructor will use the Manhattan distance.
           *
           * The validCellToQueue overridable-function allows for deriving classes to limit the queue traversal
           * to a subset of all costmap cells. LimitedCostmapQueue does this by ignoring distances above a limit.
           *
           */
          class CostmapQueue : public MapBasedQueue<CellData>
          {
          public:
           /**
           * @brief constructor
           * @param costmap Costmap which defines the size/number of cells
           * @param manhattan If true,   sort cells by Manhattan distance,   otherwise use Euclidean distance
           */
           explicit CostmapQueue(  nav2_costmap_2d::Costmap2D & costmap,   bool manhattan = false );
          
           /**
           * @brief Clear the queue
           */
           void reset(   ) override;
          
           /**
           * @brief Add a cell the queue
           * @param x X coordinate of the cell
           * @param y Y coordinate of the cell
           */
           void enqueueCell(  unsigned int x,   unsigned int y );
          
           /**
           * @brief Get the next cell to examine,   and enqueue its neighbors as needed
           * @return The next cell
           *
           * NB: Assumes that isEmpty has been called before this call and returned false
           */
           CellData getNextCell(   );
          
           /**
           * @brief Check to see if we should add this cell to the queue. Always true unless overridden.
           * @param cell The cell to check
           * @return True,   unless overriden
           */
           virtual bool validCellToQueue(  const CellData & /*cell*/ ) {return true;}
           /**
           * @brief convenience typedef for a pointer
           */
           typedef std::shared_ptr<CostmapQueue> Ptr;
          
          protected:
           /**
           * @brief Enqueue a cell with the given coordinates and the given source cell
           */
           void enqueueCell(  
           unsigned int index,   unsigned int cur_x,   unsigned int cur_y,   unsigned int src_x,  
           unsigned int src_y );
          
           /**
           * @brief Compute the cached distances
           */
           void computeCache(   );
          
           nav2_costmap_2d::Costmap2D & costmap_;
           std::vector<bool> seen_;
           int max_distance_;
           bool manhattan_;
          
          protected:
           /**
           * @brief Lookup pre-computed distances
           * @param cur_x The x coordinate of the current cell
           * @param cur_y The y coordinate of the current cell
           * @param src_x The x coordinate of the source cell
           * @param src_y The y coordinate of the source cell
           * @return
           */
           inline double distanceLookup(  
           const unsigned int cur_x,   const unsigned int cur_y,  
           const unsigned int src_x,   const unsigned int src_y )
           {
           unsigned int dx = CellData::absolute_difference(  cur_x,   src_x );
           unsigned int dy = CellData::absolute_difference(  cur_y,   src_y );
           return cached_distances_[dx][dy];
           }
           std::vector<std::vector<double>> cached_distances_;
           int cached_max_distance_;
          };
          } // namespace costmap_queue
          
          #endif // COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
          #define COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
          
          #include "costmap_queue/costmap_queue.hpp"
          
          namespace costmap_queue
          {
          
          /**
           * @class LimitedCostmapQueue
           * @brief Extension of Costmap Queue where distances are limited to a given distance from source cells.
           */
      47  class LimitedCostmapQueue : public CostmapQueue
          {
          public:
           /**
           * @brief Constructor with limit as an integer number of cells.
           */
      53   LimitedCostmapQueue(  nav2_costmap_2d::Costmap2D & costmap,   const int cell_distance_limit );
           bool validCellToQueue(  const CellData & cell ) override;
          };
          } // namespace costmap_queue
          
          #endif // COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/map_based_queue.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
          #define COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
          
          #include <algorithm>
          #include <map>
          #include <stdexcept>
          #include <utility>
          #include <vector>
          
          namespace costmap_queue
          {
          /**
           * @brief Templatized interface for a priority queue
           *
           * This is faster than the std::priority_queue implementation in certain cases because iterating does
           * not require resorting after every element is examined.
           * Based on https://github.com/ros-planning/navigation/pull/525
           * The relative speed of this against the priority queue depends how many items with each
           * priority are inserted into the queue.
           *
           * One additional speed up depends on the patterns of priorities during each iteration of the queue.
           * If the same priorities are inserted into the queue on every iteration,   then it is quicker to
           * set reset_bins = false,   such that the priority bins are not reset and will not have to be recreated
           * on each iteration.
           */
          template<class item_t>
      61  class MapBasedQueue
          {
          public:
           /**
           * @brief Default Constructor
           */
      67   explicit MapBasedQueue(  bool reset_bins = true )
           : reset_bins_(  reset_bins ),   item_count_(  0 )
           {
           reset(   );
           }
          
           /**
           * @brief Clear the queue
           */
      76   virtual void reset(   )
           {
           if (  reset_bins_ || item_count_ > 0 ) {
           item_bins_.clear(   );
           item_count_ = 0;
           }
           iter_ = last_insert_iter_ = item_bins_.end(   );
           }
          
           /**
           * @brief Add a new item to the queue with a set priority
           * @param priority Priority of the item
           * @param item Payload item
           */
      90   void enqueue(  const double priority,   item_t item )
           {
           // We keep track of the last priority we inserted. If this items priority
           // matches the previous insertion we can avoid searching through all the
           // bins.
           if (  last_insert_iter_ == item_bins_.end(   ) || last_insert_iter_->first != priority ) {
           last_insert_iter_ = item_bins_.find(  priority );
          
           // If not found,   create a new bin
           if (  last_insert_iter_ == item_bins_.end(   ) ) {
           auto map_item = std::make_pair(  priority,   std::move(  std::vector<item_t>(   ) ) );
          
           // Inserts an item if it doesn't exist. Returns an iterator to the item
           // whether it existed or was inserted.
           std::pair<ItemMapIterator,   bool> insert_result = item_bins_.insert(  std::move(  map_item ) );
           last_insert_iter_ = insert_result.first;
           }
           }
          
           // Add the item to the vector for this map key
           last_insert_iter_->second.push_back(  item );
           item_count_++;
          
           // Use short circuiting to check if we want to update the iterator
           if (  iter_ == item_bins_.end(   ) || priority < iter_->first ) {
           iter_ = last_insert_iter_;
           }
           }
          
           /**
           * @brief Check to see if there is anything in the queue
           * @return True if there is nothing in the queue
           *
           * Must be called prior to front/pop.
           */
     125   bool isEmpty(   )
           {
           return item_count_ == 0;
           }
          
           /**
           * @brief Return the item at the front of the queue
           * @return The item at the front of the queue
           */
     134   item_t & front(   )
           {
           if (  iter_ == item_bins_.end(   ) ) {
           throw std::out_of_range(  "front(   ) called on empty costmap_queue::MapBasedQueue!" );
           }
          
           return iter_->second.back(   );
           }
          
           /**
           * @brief Remove (  and destroy ) the item at the front of the queue
           */
     146   void pop(   )
           {
           if (  iter_ != item_bins_.end(   ) && !iter_->second.empty(   ) ) {
           iter_->second.pop_back(   );
           item_count_--;
           }
          
           auto not_empty = [](  const typename ItemMap::value_type & key_val ) {
           return !key_val.second.empty(   );
           };
           iter_ = std::find_if(  iter_,   item_bins_.end(   ),   not_empty );
           }
          
          protected:
           using ItemMap = std::map<double,   std::vector<item_t>>;
           using ItemMapIterator = typename ItemMap::iterator;
          
           bool reset_bins_;
          
           ItemMap item_bins_;
           unsigned int item_count_;
           ItemMapIterator iter_;
           ItemMapIterator last_insert_iter_;
          };
          } // namespace costmap_queue
          
          #endif // COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_

navigation2/nav2_dwb_controller/costmap_queue/src/costmap_queue.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #include "costmap_queue/costmap_queue.hpp"
          #include <algorithm>
          #include <cmath>
          #include <vector>
          
          using std::hypot;
          
          namespace costmap_queue
          {
          
      44  CostmapQueue::CostmapQueue(  nav2_costmap_2d::Costmap2D & costmap,   bool manhattan )
          : MapBasedQueue(   ),   costmap_(  costmap ),   max_distance_(  -1 ),   manhattan_(  manhattan ),  
           cached_max_distance_(  -1 )
          {
           reset(   );
          }
          
      51  void CostmapQueue::reset(   )
          {
           unsigned int size_x = costmap_.getSizeInCellsX(   ),   size_y = costmap_.getSizeInCellsY(   );
           if (  seen_.size(   ) != size_x * size_y ) {
           seen_.resize(  size_x * size_y );
           }
           std::fill(  seen_.begin(   ),   seen_.end(   ),   false );
           computeCache(   );
           MapBasedQueue::reset(   );
          }
          
      62  void CostmapQueue::enqueueCell(  unsigned int x,   unsigned int y )
          {
           unsigned int index = costmap_.getIndex(  x,   y );
           enqueueCell(  index,   x,   y,   x,   y );
          }
          
      68  void CostmapQueue::enqueueCell(  
           unsigned int index,   unsigned int cur_x,   unsigned int cur_y,  
           unsigned int src_x,   unsigned int src_y )
          {
           if (  seen_[index] ) {return;}
          
           // we compute our distance table one cell further than the inflation radius
           // dictates so we can make the check below
           double distance = distanceLookup(  cur_x,   cur_y,   src_x,   src_y );
           CellData data(  distance,   index,   cur_x,   cur_y,   src_x,   src_y );
           if (  validCellToQueue(  data ) ) {
           seen_[index] = true;
           enqueue(  distance,   data );
           }
          }
          
      84  CellData CostmapQueue::getNextCell(   )
          {
           // get the highest priority cell and pop it off the priority queue
           CellData current_cell = front(   );
           pop(   );
          
           unsigned int index = current_cell.index_;
           unsigned int mx = current_cell.x_;
           unsigned int my = current_cell.y_;
           unsigned int sx = current_cell.src_x_;
           unsigned int sy = current_cell.src_y_;
          
           // attempt to put the neighbors of the current cell onto the queue
           unsigned int size_x = costmap_.getSizeInCellsX(   );
           if (  mx > 0 ) {
           enqueueCell(  index - 1,   mx - 1,   my,   sx,   sy );
           }
           if (  my > 0 ) {
           enqueueCell(  index - size_x,   mx,   my - 1,   sx,   sy );
           }
           if (  mx < size_x - 1 ) {
           enqueueCell(  index + 1,   mx + 1,   my,   sx,   sy );
           }
           if (  my < costmap_.getSizeInCellsY(   ) - 1 ) {
           enqueueCell(  index + size_x,   mx,   my + 1,   sx,   sy );
           }
          
           return current_cell;
          }
          
     114  void CostmapQueue::computeCache(   )
          {
           if (  max_distance_ == -1 ) {
           max_distance_ = std::max(  costmap_.getSizeInCellsX(   ),   costmap_.getSizeInCellsY(   ) );
           }
           if (  max_distance_ == cached_max_distance_ ) {return;}
           cached_distances_.clear(   );
          
           cached_distances_.resize(  max_distance_ + 2 );
          
           for (  unsigned int i = 0; i < cached_distances_.size(   ); ++i ) {
           cached_distances_[i].resize(  max_distance_ + 2 );
           for (  unsigned int j = 0; j < cached_distances_[i].size(   ); ++j ) {
           if (  manhattan_ ) {
           cached_distances_[i][j] = i + j;
           } else {
           cached_distances_[i][j] = hypot(  i,   j );
           }
           }
           }
           cached_max_distance_ = max_distance_;
          }
          
          } // namespace costmap_queue

navigation2/nav2_dwb_controller/costmap_queue/src/limited_costmap_queue.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "costmap_queue/limited_costmap_queue.hpp"
          
          namespace costmap_queue
          {
          
      40  LimitedCostmapQueue::LimitedCostmapQueue(  
      41   nav2_costmap_2d::Costmap2D & costmap,  
           const int distance_limit )
          : CostmapQueue(  costmap )
          {
           max_distance_ = distance_limit;
           reset(   );
          }
          
      49  bool LimitedCostmapQueue::validCellToQueue(  const CellData & cell )
          {
           return cell.distance_ <= max_distance_;
          }
          
          } // namespace costmap_queue

navigation2/nav2_dwb_controller/costmap_queue/test/mbq_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <string>
          #include "gtest/gtest.h"
          #include "costmap_queue/map_based_queue.hpp"
          
          using costmap_queue::MapBasedQueue;
          
      41  void letter_test(  MapBasedQueue<char> & q,   const char test_letter )
          {
           ASSERT_FALSE(  q.isEmpty(   ) );
           char c = q.front(   );
           EXPECT_EQ(  c,   test_letter );
           q.pop(   );
          }
          
      49  TEST(  MapBasedQueue,   emptyQueue )
          {
           MapBasedQueue<char> q;
           EXPECT_TRUE(  q.isEmpty(   ) );
           q.enqueue(  1.0,   'A' );
           EXPECT_FALSE(  q.isEmpty(   ) );
          }
          
      57  TEST(  MapBasedQueue,   checkOrdering )
          {
           MapBasedQueue<char> q;
           q.enqueue(  1.0,   'A' );
           q.enqueue(  3.0,   'B' );
           q.enqueue(  2.0,   'C' );
           q.enqueue(  5.0,   'D' );
           q.enqueue(  0.0,   'E' );
          
           std::string expected = "EACBD";
           for (  unsigned int i = 0; i < expected.size(   ); i++ ) {
           letter_test(  q,   expected[i] );
           }
           EXPECT_TRUE(  q.isEmpty(   ) );
          }
          
      73  TEST(  MapBasedQueue,   checkDynamicOrdering )
          {
           MapBasedQueue<char> q;
           q.enqueue(  1.0,   'A' );
           q.enqueue(  3.0,   'B' );
           q.enqueue(  2.0,   'C' );
           q.enqueue(  5.0,   'D' );
          
           std::string expected = "ACB";
           for (  unsigned int i = 0; i < expected.size(   ); i++ ) {
           letter_test(  q,   expected[i] );
           }
          
           q.enqueue(  0.0,   'E' );
           letter_test(  q,   'E' );
          }
          
      90  TEST(  MapBasedQueue,   checkDynamicOrdering2 )
          {
           MapBasedQueue<char> q;
           q.enqueue(  1.0,   'A' );
           q.enqueue(  2.0,   'B' );
           letter_test(  q,   'A' );
           q.enqueue(  3.0,   'C' );
           letter_test(  q,   'B' );
          }
          
     100  TEST(  MapBasedQueue,   checkDynamicOrdering3 )
          {
           MapBasedQueue<char> q;
           q.enqueue(  1.0,   'A' );
           q.enqueue(  2.0,   'B' );
           q.enqueue(  5.0,   'D' );
           letter_test(  q,   'A' );
           letter_test(  q,   'B' );
           q.enqueue(  1.0,   'C' );
           letter_test(  q,   'C' );
           letter_test(  q,   'D' );
          }
          
     113  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_dwb_controller/costmap_queue/test/utest.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <cmath>
          #include <memory>
          #include <algorithm>
          #include "gtest/gtest.h"
          #include "costmap_queue/costmap_queue.hpp"
          #include "costmap_queue/limited_costmap_queue.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          using std::hypot;
          
      45  nav2_costmap_2d::Costmap2D costmap(  5,   5,   1.0,   0.0,   0.0 );
          
      47  TEST(  CostmapQueue,   basicQueue )
          {
           costmap_queue::CostmapQueue q(  costmap );
           int count = 0;
           q.enqueueCell(  0,   0 );
           while (  !q.isEmpty(   ) ) {
           costmap_queue::CellData cell = q.getNextCell(   );
           EXPECT_EQ(  cell.distance_,   hypot(  cell.x_,   cell.y_ ) );
           count++;
           }
           EXPECT_EQ(  count,   25 );
          }
          
      60  TEST(  CostmapQueue,   bigTest )
          {
           nav2_costmap_2d::Costmap2D big_map(  500,   500,   1.0,   0.0,   0.0 );
           costmap_queue::CostmapQueue q(  big_map );
           int count = 0;
           q.enqueueCell(  0,   0 );
           while (  !q.isEmpty(   ) ) {
           costmap_queue::CellData cell = q.getNextCell(   );
           EXPECT_EQ(  cell.distance_,   hypot(  cell.x_,   cell.y_ ) );
           count++;
           }
           EXPECT_EQ(  count,   500 * 500 );
          }
          
      74  TEST(  CostmapQueue,   linearQueue )
          {
           costmap_queue::CostmapQueue q(  costmap );
           int count = 0;
           q.enqueueCell(  0,   0 );
           q.enqueueCell(  0,   1 );
           q.enqueueCell(  0,   2 );
           q.enqueueCell(  0,   3 );
           q.enqueueCell(  0,   4 );
           while (  !q.isEmpty(   ) ) {
           costmap_queue::CellData cell = q.getNextCell(   );
           EXPECT_EQ(  cell.distance_,   cell.x_ );
           count++;
           }
           EXPECT_EQ(  count,   25 );
          }
          
      91  TEST(  CostmapQueue,   crossQueue )
          {
           costmap_queue::CostmapQueue q(  costmap );
           int count = 0;
           int xs[] = {1,   2,   2,   3};
           int ys[] = {2,   1,   3,   2};
           int N = 4;
           for (  int i = 0; i < N; i++ ) {
           q.enqueueCell(  xs[i],   ys[i] );
           }
          
           while (  !q.isEmpty(   ) ) {
           costmap_queue::CellData cell = q.getNextCell(   );
           double min_d = 1000;
          
           for (  int i = 0; i < N; i++ ) {
           double dd = hypot(  xs[i] - static_cast<float>(  cell.x_ ),   ys[i] - static_cast<float>(  cell.y_ ) );
           min_d = std::min(  min_d,   dd );
           }
           EXPECT_NEAR(  cell.distance_,   min_d,   0.00001 );
           count++;
           }
           EXPECT_EQ(  count,   25 );
          }
          
     116  TEST(  CostmapQueue,   limitedQueue )
          {
           costmap_queue::LimitedCostmapQueue q(  costmap,   5 );
           int count = 0;
           q.enqueueCell(  0,   0 );
           while (  !q.isEmpty(   ) ) {
           costmap_queue::CellData cell = q.getNextCell(   );
           EXPECT_EQ(  cell.distance_,   hypot(  cell.x_,   cell.y_ ) );
           count++;
           }
           EXPECT_EQ(  count,   24 );
          
           costmap_queue::LimitedCostmapQueue q2(  costmap,   3 );
           count = 0;
           q2.enqueueCell(  0,   0 );
           while (  !q2.isEmpty(   ) ) {
           q2.getNextCell(   );
           count++;
           }
           EXPECT_EQ(  count,   11 );
          }
          
     138  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__DWB_LOCAL_PLANNER_HPP_
          #define DWB_CORE__DWB_LOCAL_PLANNER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_core/controller.hpp"
          #include "nav2_core/goal_checker.hpp"
          #include "dwb_core/publisher.hpp"
          #include "dwb_core/trajectory_critic.hpp"
          #include "dwb_core/trajectory_generator.hpp"
          #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
          #include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          namespace dwb_core
          {
          
          /**
           * @class DWBLocalPlanner
           * @brief Plugin-based flexible controller
           */
      61  class DWBLocalPlanner : public nav2_core::Controller
          {
          public:
           /**
           * @brief Constructor that brings up pluginlib loaders
           */
      67   DWBLocalPlanner(   );
          
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           virtual ~DWBLocalPlanner(   ) {}
          
           /**
           * @brief Activate lifecycle node
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate lifecycle node
           */
      84   void deactivate(   ) override;
          
           /**
           * @brief Cleanup lifecycle node
           */
      89   void cleanup(   ) override;
          
           /**
           * @brief nav2_core setPlan - Sets the global plan
           * @param path The global plan
           */
      95   void setPlan(  const nav_msgs::msg::Path & path ) override;
          
           /**
           * @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
           *
           * It is presumed that the global plan is already set.
           *
           * This is mostly a wrapper for the protected computeVelocityCommands
           * function which has additional debugging info.
           *
           * @param pose Current robot pose
           * @param velocity Current robot velocity
           * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
           * @return The best command for the robot to drive
           */
     110   geometry_msgs::msg::TwistStamped computeVelocityCommands(  
           const geometry_msgs::msg::PoseStamped & pose,  
           const geometry_msgs::msg::Twist & velocity,  
           nav2_core::GoalChecker * /*goal_checker*/ ) override;
          
           /**
           * @brief Score a given command. Can be used for testing.
           *
           * Given a trajectory,   calculate the score where lower scores are better.
           * If the given (  positive ) score exceeds the best_score,   calculation may be cut short,   as the
           * score can only go up from there.
           *
           * @param traj Trajectory to check
           * @param best_score If positive,   the threshold for early termination
           * @return The full scoring of the input trajectory
           */
     126   virtual dwb_msgs::msg::TrajectoryScore scoreTrajectory(  
           const dwb_msgs::msg::Trajectory2D & traj,  
           double best_score = -1 );
          
           /**
           * @brief Compute the best command given the current pose and velocity,   with possible debug information
           *
           * Same as above computeVelocityCommands,   but with debug results.
           * If the results pointer is not null,   additional information about the twists
           * evaluated will be in results after the call.
           *
           * @param pose Current robot pose
           * @param velocity Current robot velocity
           * @param results Output param,   if not NULL,   will be filled in with full evaluation results
           * @return Best command
           */
           virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands(  
           const nav_2d_msgs::msg::Pose2DStamped & pose,  
           const nav_2d_msgs::msg::Twist2D & velocity,  
           std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results );
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
           void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) override
           {
           if (  traj_generator_ ) {
           traj_generator_->setSpeedLimit(  speed_limit,   percentage );
           }
           }
          
          protected:
           /**
           * @brief Helper method for two common operations for the operating on the global_plan
           *
           * Transforms the global plan (  stored in global_plan_ ) relative to the pose and saves it in
           * transformed_plan and possibly publishes it. Then it takes the last pose and transforms it
           * to match the local costmap's frame
           */
           void prepareGlobalPlan(  
           const nav_2d_msgs::msg::Pose2DStamped & pose,   nav_2d_msgs::msg::Path2D & transformed_plan,  
           nav_2d_msgs::msg::Pose2DStamped & goal_pose,   bool publish_plan = true );
          
           /**
           * @brief Iterate through all the twists and find the best one
           */
           virtual dwb_msgs::msg::TrajectoryScore coreScoringAlgorithm(  
           const geometry_msgs::msg::Pose2D & pose,  
           const nav_2d_msgs::msg::Twist2D velocity,  
           std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results );
          
           /**
           * @brief Transforms global plan into same frame as pose,   clips far away poses and possibly prunes passed poses
           *
           * Three key operations
           * 1 ) Transforms global plan into frame of the given pose
           * 2 ) Only returns poses that are near the robot,   i.e. whether they are likely on the local costmap
           * 3 ) If prune_plan_ is true,   it will remove all points that we've already passed from both the transformed plan
           * and the saved global_plan_. Technically,   it iterates to a pose on the path that is within prune_distance_
           * of the robot and erases all poses before that.
           *
           * Additionally,   shorten_transformed_plan_ determines whether we will pass the full plan all
           * the way to the nav goal on to the critics or just a subset of the plan near the robot.
           * True means pass just a subset. This gives DWB less discretion to decide how it gets to the
           * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner.
           */
           virtual nav_2d_msgs::msg::Path2D transformGlobalPlan(  
           const nav_2d_msgs::msg::Pose2DStamped & pose );
           nav_2d_msgs::msg::Path2D global_plan_; ///< Saved Global Plan
           bool prune_plan_;
           double prune_distance_;
           bool debug_trajectory_details_;
           rclcpp::Duration transform_tolerance_{0,   0};
           bool shorten_transformed_plan_;
          
           /**
           * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
           *
           * @param base_name The name of the critic as read in from the parameter server
           * @return Our attempted resolution of the name,   with namespace prepended and/or the suffix Critic appended
           */
           std::string resolveCriticClassName(  std::string base_name );
          
           /**
           * @brief Load the critic parameters from the namespace
           * @param name The namespace of this planner.
           */
           virtual void loadCritics(   );
          
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
           rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "DWBLocalPlanner" )};
          
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
          
           std::unique_ptr<DWBPublisher> pub_;
           std::vector<std::string> default_critic_namespaces_;
          
           // Plugin handling
           pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
           TrajectoryGenerator::Ptr traj_generator_;
          
           pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
           std::vector<TrajectoryCritic::Ptr> critics_;
          
           std::string dwb_plugin_name_;
          
           bool short_circuit_trajectory_evaluation_;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__DWB_LOCAL_PLANNER_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CORE__EXCEPTIONS_HPP_
          #define DWB_CORE__EXCEPTIONS_HPP_
          
          #include <stdexcept>
          #include <string>
          #include <memory>
          
          #include "nav2_core/exceptions.hpp"
          
          namespace dwb_core
          {
          
          /**
           * @class PlannerTFException
           * @brief Thrown when the planner cannot complete its operation due to TF errors
           */
      50  class PlannerTFException : public nav2_core::PlannerException
          {
          public:
      53   explicit PlannerTFException(  const std::string description )
           : nav2_core::PlannerException(  description ) {}
          };
          
          /**
           * @class IllegalTrajectoryException
           * @brief Thrown when one of the critics encountered a fatal error
           */
      61  class IllegalTrajectoryException : public nav2_core::PlannerException
          {
          public:
      64   IllegalTrajectoryException(  const std::string critic_name,   const std::string description )
           : nav2_core::PlannerException(  description ),   critic_name_(  critic_name ) {}
      66   std::string getCriticName(   ) const {return critic_name_;}
          
          protected:
      69   std::string critic_name_;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__EXCEPTIONS_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
          #define DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
          
          #include <map>
          #include <utility>
          #include <string>
          #include "dwb_core/exceptions.hpp"
          #include "nav2_core/exceptions.hpp"
          
          namespace dwb_core
          {
      46  class IllegalTrajectoryTracker
          {
          public:
      49   IllegalTrajectoryTracker(   )
           : legal_count_(  0 ),   illegal_count_(  0 ) {}
          
      52   void addIllegalTrajectory(  const IllegalTrajectoryException & e );
      53   void addLegalTrajectory(   );
          
      55   std::map<std::pair<std::string,   std::string>,   double> getPercentages(   ) const;
          
      57   std::string getMessage(   ) const;
          
          protected:
      60   std::map<std::pair<std::string,   std::string>,   unsigned int> counts_;
           unsigned int legal_count_,   illegal_count_;
          };
          
          /**
           * @class NoLegalTrajectoriesException
           * @brief Thrown when all the trajectories explored are illegal
           */
      68  class NoLegalTrajectoriesException
      69   : public nav2_core::PlannerException
          {
          public:
      72   explicit NoLegalTrajectoriesException(  const IllegalTrajectoryTracker & tracker )
           : PlannerException(  tracker.getMessage(   ) ),  
           tracker_(  tracker ) {}
      75   IllegalTrajectoryTracker tracker_;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__PUBLISHER_HPP_
          #define DWB_CORE__PUBLISHER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "dwb_core/trajectory_critic.hpp"
          #include "dwb_msgs/msg/local_plan_evaluation.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "sensor_msgs/msg/point_cloud2.hpp"
          #include "visualization_msgs/msg/marker_array.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "builtin_interfaces/msg/duration.hpp"
          
          using rclcpp_lifecycle::LifecyclePublisher;
          
          namespace dwb_core
          {
          
          /**
           * @class DWBPublisher
           * @brief Consolidation of all the publishing logic for the DWB Local Planner.
           *
           * Right now,   it can publish
           * 1 ) The Global Plan (  as passed in using setPath )
           * 2 ) The Local Plan (  after it is calculated )
           * 3 ) The Transformed Global Plan (  since it may be different than the global )
           * 4 ) The Full LocalPlanEvaluation
           * 5 ) Markers representing the different trajectories evaluated
           * 6 ) The CostGrid (  in the form of a complex PointCloud2 )
           */
      69  class DWBPublisher
          {
          public:
      72   explicit DWBPublisher(  
      73   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      74   const std::string & plugin_name );
          
      76   nav2_util::CallbackReturn on_configure(   );
      77   nav2_util::CallbackReturn on_activate(   );
      78   nav2_util::CallbackReturn on_deactivate(   );
      79   nav2_util::CallbackReturn on_cleanup(   );
          
           /**
           * @brief Does the publisher require that the LocalPlanEvaluation be saved
           * @return True if the Evaluation is needed to publish either directly or as trajectories
           */
      85   bool shouldRecordEvaluation(   ) {return publish_evaluation_ || publish_trajectories_;}
          
           /**
           * @brief If the pointer is not null,   publish the evaluation and trajectories as needed
           */
      90   void publishEvaluation(  std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results );
      91   void publishLocalPlan(  
      92   const std_msgs::msg::Header & header,  
      93   const dwb_msgs::msg::Trajectory2D & traj );
      94   void publishCostGrid(  
      95   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,  
      96   const std::vector<TrajectoryCritic::Ptr> critics );
      97   void publishGlobalPlan(  const nav_2d_msgs::msg::Path2D plan );
      98   void publishTransformedPlan(  const nav_2d_msgs::msg::Path2D plan );
      99   void publishLocalPlan(  const nav_2d_msgs::msg::Path2D plan );
          
          protected:
     102   void publishTrajectories(  const dwb_msgs::msg::LocalPlanEvaluation & results );
          
           // Helper function for publishing other plans
     105   void publishGenericPlan(  
     106   const nav_2d_msgs::msg::Path2D plan,  
     107   rclcpp::Publisher<nav_msgs::msg::Path> & pub,   bool flag );
          
           // Flags for turning on/off publishing specific components
     110   bool publish_evaluation_;
     111   bool publish_global_plan_;
     112   bool publish_transformed_;
     113   bool publish_local_plan_;
     114   bool publish_trajectories_;
     115   bool publish_cost_grid_pc_;
     116   bool publish_input_params_;
          
           // Marker Lifetime
     119   builtin_interfaces::msg::Duration marker_lifetime_;
          
           // Publisher Objects
           std::shared_ptr<LifecyclePublisher<dwb_msgs::msg::LocalPlanEvaluation>> eval_pub_;
           std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;
           std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_;
           std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_;
           std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
           std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_;
          
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
           rclcpp::Clock::SharedPtr clock_;
           std::string plugin_name_;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__PUBLISHER_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__TRAJECTORY_CRITIC_HPP_
          #define DWB_CORE__TRAJECTORY_CRITIC_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <utility>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "nav_2d_msgs/msg/twist2_d.hpp"
          #include "nav_2d_msgs/msg/path2_d.hpp"
          #include "dwb_msgs/msg/trajectory2_d.hpp"
          #include "sensor_msgs/msg/point_cloud.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_core
          {
          /**
           * @class TrajectoryCritic
           * @brief Evaluates a Trajectory2D to produce a score
           *
           * This class defines the plugin interface for the TrajectoryCritic which
           * gives scores to trajectories,   where lower numbers are better,   but negative
           * scores are considered invalid.
           *
           * The general lifecycle is
           * 1 ) initialize is called once at the beginning which in turn calls onInit.
           * Derived classes may override onInit to load parameters as needed.
           * 2 ) prepare is called once before each set of trajectories.
           * It is presumed that there are multiple trajectories that we want to evaluate,  
           * and there may be some shared work that can be done beforehand to optimize
           * the scoring of each individual trajectory.
           * 3 ) scoreTrajectory is called once per trajectory and returns the score.
           * 4 ) debrief is called after each set of trajectories with the chosen trajectory.
           * This can be used for stateful critics that monitor the trajectory through time.
           *
           * Optionally,   there is also a debugging mechanism for certain types of critics in the
           * addCriticVisualization method. If the score for a trajectory depends on its relationship to
           * the costmap,   addCriticVisualization can provide that information to the dwb_core
           * which will publish the grid scores as a PointCloud2.
           */
      78  class TrajectoryCritic
          {
          public:
           using Ptr = std::shared_ptr<dwb_core::TrajectoryCritic>;
          
           virtual ~TrajectoryCritic(   ) {}
          
           /**
           * @brief Initialize the critic with appropriate pointers and parameters
           *
           * The name and costmap are stored as member variables.
           * A NodeHandle is created using the combination of the parent namespace and the critic name
           *
           * @param name The name of this critic
           * @param parent_namespace The namespace of the planner
           * @param costmap_ros Pointer to the costmap
           */
           void initialize(  
           const nav2_util::LifecycleNode::SharedPtr & nh,  
           const std::string & name,  
           const std::string & ns,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
           {
           node_ = nh;
     102   name_ = name;
     103   costmap_ros_ = costmap_ros;
           dwb_plugin_name_ = ns;
           if (  !nh->has_parameter(  dwb_plugin_name_ + "." + name_ + ".scale" ) ) {
           nh->declare_parameter(  
           dwb_plugin_name_ + "." + name_ + ".scale",  
           rclcpp::ParameterValue(  1.0 ) );
           }
           nh->get_parameter(  dwb_plugin_name_ + "." + name_ + ".scale",   scale_ );
           onInit(   );
           }
           virtual void onInit(   ) {}
          
           /**
           * @brief Reset the state of the critic
           *
           * Reset is called when the planner receives a new global plan.
           * This can be used to discard information specific to one plan.
           */
           virtual void reset(   ) {}
          
           /**
           * @brief Prior to evaluating any trajectories,   look at contextual information constant across all trajectories
           *
           * Subclasses may overwrite. Return false in case there is any error.
           *
           * @param pose Current pose (  costmap frame )
           * @param vel Current velocity
           * @param goal The final goal (  costmap frame )
           * @param global_plan Transformed global plan in costmap frame,   possibly cropped to nearby points
           */
           virtual bool prepare(  
           const geometry_msgs::msg::Pose2D &,   const nav_2d_msgs::msg::Twist2D &,  
           const geometry_msgs::msg::Pose2D &,  
           const nav_2d_msgs::msg::Path2D & )
           {
           return true;
           }
          
           /**
           * @brief Return a raw score for the given trajectory.
           *
           * scores < 0 are considered invalid/errors,   such as collisions
           * This is the raw score in that the scale should not be applied to it.
           */
           virtual double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) = 0;
          
           /**
           * @brief debrief informs the critic what the chosen cmd_vel was (  if it cares )
           */
           virtual void debrief(  const nav_2d_msgs::msg::Twist2D & ) {}
          
           /**
           * @brief Add information to the given pointcloud for debugging costmap-grid based scores
           *
           * addCriticVisualization is an optional debugging mechanism for providing rich information
           * about the cost for certain trajectories. Some critics will have scoring mechanisms
           * wherein there will be some score for each cell in the costmap. This could be as
           * straightforward as the cost in the costmap,   or it could be the number of cells away
           * from the goal pose.
           *
           * Prior to calling this,   dwb_core will load the PointCloud's header and the points
           * in row-major order. The critic may then add a ChannelFloat to the channels member of the PC
           * with the same number of values as the points array. This information may then be converted
           * and published as a PointCloud2.
           *
           * @param pc PointCloud to add channels to
           */
           virtual void addCriticVisualization(  std::vector<std::pair<std::string,   std::vector<float>>> & ) {}
          
           std::string getName(   )
           {
           return name_;
           }
          
           virtual double getScale(   ) const {return scale_;}
           void setScale(  const double scale ) {scale_ = scale;}
          
          protected:
           std::string name_;
           std::string dwb_plugin_name_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
           double scale_;
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__TRAJECTORY_CRITIC_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__TRAJECTORY_GENERATOR_HPP_
          #define DWB_CORE__TRAJECTORY_GENERATOR_HPP_
          
          #include <vector>
          #include <string>
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "nav_2d_msgs/msg/twist2_d.hpp"
          #include "dwb_msgs/msg/trajectory2_d.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_core
          {
          
          /**
           * @class TrajectoryGenerator
           * @brief Interface for iterating through possible velocities and creating trajectories
           *
           * This class defines the plugin interface for two separate but related components.
           *
           * First,   this class provides an iterator interface for exploring all of the velocities
           * to search,   given the current velocity.
           *
           * Second,   the class gives an independent interface for creating a trajectory from a twist,  
           * i.e. projecting it out in time and space.
           *
           * Both components rely heavily on the robot's kinematic model,   and can share many parameters,  
           * which is why they are grouped into a singular class.
           */
      64  class TrajectoryGenerator
          {
          public:
           typedef std::shared_ptr<dwb_core::TrajectoryGenerator> Ptr;
          
      69   virtual ~TrajectoryGenerator(   ) {}
          
           /**
           * @brief Initialize parameters as needed
           * @param nh NodeHandle to read parameters from
           */
      75   virtual void initialize(  
      76   const nav2_util::LifecycleNode::SharedPtr & nh,  
      77   const std::string & plugin_name ) = 0;
      78   virtual void reset(   ) {}
           /**
           * @brief Start a new iteration based on the current velocity
           * @param current_velocity
           */
      83   virtual void startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity ) = 0;
          
           /**
           * @brief Test to see whether there are more twists to test
           * @return True if more twists,   false otherwise
           */
      89   virtual bool hasMoreTwists(   ) = 0;
          
           /**
           * @brief Return the next twist and advance the iteration
           * @return The Twist!
           */
      95   virtual nav_2d_msgs::msg::Twist2D nextTwist(   ) = 0;
          
           /**
           * @brief Get all the twists for an iteration.
           *
           * Note: Resets the iterator if one is in process
           *
           * @param current_velocity
           * @return all the twists
           */
     105   virtual std::vector<nav_2d_msgs::msg::Twist2D> getTwists(  
     106   const nav_2d_msgs::msg::Twist2D & current_velocity )
           {
           std::vector<nav_2d_msgs::msg::Twist2D> twists;
           startNewIteration(  current_velocity );
           while (  hasMoreTwists(   ) ) {
           twists.push_back(  nextTwist(   ) );
           }
           return twists;
           }
          
           /**
           * @brief Given a cmd_vel in the robot's frame and initial conditions,   generate a Trajectory2D
           * @param start_pose Current robot location
           * @param start_vel Current robot velocity
           * @param cmd_vel The desired command velocity
           */
     122   virtual dwb_msgs::msg::Trajectory2D generateTrajectory(  
     123   const geometry_msgs::msg::Pose2D & start_pose,  
     124   const nav_2d_msgs::msg::Twist2D & start_vel,  
     125   const nav_2d_msgs::msg::Twist2D & cmd_vel ) = 0;
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
     134   virtual void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) = 0;
          };
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__TRAJECTORY_GENERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_utils.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CORE__TRAJECTORY_UTILS_HPP_
          #define DWB_CORE__TRAJECTORY_UTILS_HPP_
          
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_msgs/msg/trajectory2_d.hpp"
          
          namespace dwb_core
          {
          
          /**
           * @brief Helper function to find a pose in the trajectory with a particular time time_offset
           * @param trajectory The trajectory to search
           * @param time_offset The desired time_offset
           * @return reference to the pose that is closest to the particular time offset
           *
           * Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,  
           * the search ends,   since the poses have increasing time_offsets.
           */
      53  const geometry_msgs::msg::Pose2D & getClosestPose(  
           const dwb_msgs::msg::Trajectory2D & trajectory,  
           const double time_offset );
          
          /**
           * @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
           * @param trajectory The trajectory with pose and time offset information
           * @param time_offset The desired time_offset
           * @return New Pose2D with interpolated values
           * @note If the given time offset is outside the bounds of the trajectory,   the return pose will be either the first or last pose.
           */
      64  geometry_msgs::msg::Pose2D projectPose(  
           const dwb_msgs::msg::Trajectory2D & trajectory,  
           const double time_offset );
          
          } // namespace dwb_core
          
          #endif // DWB_CORE__TRAJECTORY_UTILS_HPP_

navigation2/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "dwb_core/dwb_local_planner.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "dwb_core/illegal_trajectory_tracker.hpp"
          #include "dwb_msgs/msg/critic_score.hpp"
          #include "nav_2d_msgs/msg/twist2_d.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav_2d_utils/parameters.hpp"
          #include "nav_2d_utils/tf_help.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "geometry_msgs/msg/twist_stamped.hpp"
          
          using nav2_util::declare_parameter_if_not_declared;
          using nav2_util::geometry_utils::euclidean_distance;
          
          namespace dwb_core
          {
          
      62  DWBLocalPlanner::DWBLocalPlanner(   )
          : traj_gen_loader_(  "dwb_core",   "dwb_core::TrajectoryGenerator" ),  
           critic_loader_(  "dwb_core",   "dwb_core::TrajectoryCritic" )
          {
          }
          
      68  void DWBLocalPlanner::configure(  
      69   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      70   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      71   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           node_ = parent;
           auto node = node_.lock(   );
          
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
           costmap_ros_ = costmap_ros;
           tf_ = tf;
           dwb_plugin_name_ = name;
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".critics",  
           rclcpp::PARAMETER_STRING_ARRAY );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".default_critic_namespaces",  
           rclcpp::ParameterValue(  std::vector<std::string>(   ) ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".prune_plan",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".prune_distance",  
           rclcpp::ParameterValue(  2.0 ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".debug_trajectory_details",  
           rclcpp::ParameterValue(  false ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".trajectory_generator_name",  
           rclcpp::ParameterValue(  std::string(  "dwb_plugins::StandardTrajectoryGenerator" ) ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".transform_tolerance",  
           rclcpp::ParameterValue(  0.1 ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".shorten_transformed_plan",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",  
           rclcpp::ParameterValue(  true ) );
          
           std::string traj_generator_name;
          
           double transform_tolerance;
           node->get_parameter(  dwb_plugin_name_ + ".transform_tolerance",   transform_tolerance );
           transform_tolerance_ = rclcpp::Duration::from_seconds(  transform_tolerance );
           RCLCPP_INFO(  logger_,   "Setting transform_tolerance to %f",   transform_tolerance );
          
           node->get_parameter(  dwb_plugin_name_ + ".prune_plan",   prune_plan_ );
           node->get_parameter(  dwb_plugin_name_ + ".prune_distance",   prune_distance_ );
           node->get_parameter(  dwb_plugin_name_ + ".debug_trajectory_details",   debug_trajectory_details_ );
           node->get_parameter(  dwb_plugin_name_ + ".trajectory_generator_name",   traj_generator_name );
           node->get_parameter(  
           dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",  
           short_circuit_trajectory_evaluation_ );
           node->get_parameter(  dwb_plugin_name_ + ".shorten_transformed_plan",   shorten_transformed_plan_ );
          
           pub_ = std::make_unique<DWBPublisher>(  node,   dwb_plugin_name_ );
           pub_->on_configure(   );
          
           traj_generator_ = traj_gen_loader_.createUniqueInstance(  traj_generator_name );
          
           traj_generator_->initialize(  node,   dwb_plugin_name_ );
          
           try {
           loadCritics(   );
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  logger_,   "Couldn't load critics! Caught exception: %s",   e.what(   ) );
           throw;
           }
          }
          
          void
     141  DWBLocalPlanner::activate(   )
          {
           pub_->on_activate(   );
          }
          
          void
     147  DWBLocalPlanner::deactivate(   )
          {
           pub_->on_deactivate(   );
          }
          
          void
     153  DWBLocalPlanner::cleanup(   )
          {
           pub_->on_cleanup(   );
          
           traj_generator_.reset(   );
          }
          
          std::string
     161  DWBLocalPlanner::resolveCriticClassName(  std::string base_name )
          {
           if (  base_name.find(  "Critic" ) == std::string::npos ) {
           base_name = base_name + "Critic";
           }
          
           if (  base_name.find(  "::" ) == std::string::npos ) {
           for (  unsigned int j = 0; j < default_critic_namespaces_.size(   ); j++ ) {
           std::string full_name = default_critic_namespaces_[j] + "::" + base_name;
           if (  critic_loader_.isClassAvailable(  full_name ) ) {
           return full_name;
           }
           }
           }
           return base_name;
          }
          
          void
     179  DWBLocalPlanner::loadCritics(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           node->get_parameter(  dwb_plugin_name_ + ".default_critic_namespaces",   default_critic_namespaces_ );
           if (  default_critic_namespaces_.empty(   ) ) {
           default_critic_namespaces_.emplace_back(  "dwb_critics" );
           }
          
           std::vector<std::string> critic_names;
           if (  !node->get_parameter(  dwb_plugin_name_ + ".critics",   critic_names ) ) {
           throw std::runtime_error(  "No critics defined for " + dwb_plugin_name_ );
           }
          
           node->get_parameter(  dwb_plugin_name_ + ".critics",   critic_names );
           for (  unsigned int i = 0; i < critic_names.size(   ); i++ ) {
           std::string critic_plugin_name = critic_names[i];
           std::string plugin_class;
          
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + "." + critic_plugin_name + ".class",  
           rclcpp::ParameterValue(  critic_plugin_name ) );
           node->get_parameter(  dwb_plugin_name_ + "." + critic_plugin_name + ".class",   plugin_class );
          
           plugin_class = resolveCriticClassName(  plugin_class );
          
           TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(  plugin_class );
           RCLCPP_INFO(  
           logger_,  
           "Using critic \"%s\" (  %s )",   critic_plugin_name.c_str(   ),   plugin_class.c_str(   ) );
           critics_.push_back(  plugin );
           try {
           plugin->initialize(  node,   critic_plugin_name,   dwb_plugin_name_,   costmap_ros_ );
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  logger_,   "Couldn't initialize critic plugin!" );
           throw;
           }
           RCLCPP_INFO(  logger_,   "Critic plugin initialized" );
           }
          }
          
          void
     224  DWBLocalPlanner::setPlan(  const nav_msgs::msg::Path & path )
          {
           auto path2d = nav_2d_utils::pathToPath2D(  path );
           for (  TrajectoryCritic::Ptr & critic : critics_ ) {
           critic->reset(   );
           }
          
           traj_generator_->reset(   );
          
           pub_->publishGlobalPlan(  path2d );
           global_plan_ = path2d;
          }
          
          geometry_msgs::msg::TwistStamped
     238  DWBLocalPlanner::computeVelocityCommands(  
     239   const geometry_msgs::msg::PoseStamped & pose,  
     240   const geometry_msgs::msg::Twist & velocity,  
     241   nav2_core::GoalChecker * /*goal_checker*/ )
          {
           std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results = nullptr;
           if (  pub_->shouldRecordEvaluation(   ) ) {
           results = std::make_shared<dwb_msgs::msg::LocalPlanEvaluation>(   );
           }
          
           try {
           nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands(  
           nav_2d_utils::poseStampedToPose2D(  pose ),  
           nav_2d_utils::twist3Dto2D(  velocity ),   results );
           pub_->publishEvaluation(  results );
           geometry_msgs::msg::TwistStamped cmd_vel;
           cmd_vel.twist = nav_2d_utils::twist2Dto3D(  cmd_vel2d.velocity );
           return cmd_vel;
           } catch (  const nav2_core::PlannerException & e ) {
           pub_->publishEvaluation(  results );
           throw;
           }
          }
          
          void
     263  DWBLocalPlanner::prepareGlobalPlan(  
     264   const nav_2d_msgs::msg::Pose2DStamped & pose,   nav_2d_msgs::msg::Path2D & transformed_plan,  
     265   nav_2d_msgs::msg::Pose2DStamped & goal_pose,   bool publish_plan )
          {
           transformed_plan = transformGlobalPlan(  pose );
           if (  publish_plan ) {
           pub_->publishTransformedPlan(  transformed_plan );
           }
          
           goal_pose.header.frame_id = global_plan_.header.frame_id;
           goal_pose.pose = global_plan_.poses.back(   );
           nav_2d_utils::transformPose(  
           tf_,   costmap_ros_->getGlobalFrameID(   ),   goal_pose,  
           goal_pose,   transform_tolerance_ );
          }
          
          nav_2d_msgs::msg::Twist2DStamped
     280  DWBLocalPlanner::computeVelocityCommands(  
     281   const nav_2d_msgs::msg::Pose2DStamped & pose,  
     282   const nav_2d_msgs::msg::Twist2D & velocity,  
     283   std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results )
          {
           if (  results ) {
           results->header.frame_id = pose.header.frame_id;
           results->header.stamp = clock_->now(   );
           }
          
           nav_2d_msgs::msg::Path2D transformed_plan;
           nav_2d_msgs::msg::Pose2DStamped goal_pose;
          
           prepareGlobalPlan(  pose,   transformed_plan,   goal_pose );
          
           nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(   );
           std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(  *(  costmap->getMutex(   ) ) );
          
           for (  TrajectoryCritic::Ptr & critic : critics_ ) {
           if (  !critic->prepare(  pose.pose,   velocity,   goal_pose.pose,   transformed_plan ) ) {
           RCLCPP_WARN(  rclcpp::get_logger(  "DWBLocalPlanner" ),   "A scoring function failed to prepare" );
           }
           }
          
           try {
           dwb_msgs::msg::TrajectoryScore best = coreScoringAlgorithm(  pose.pose,   velocity,   results );
          
           // Return Value
           nav_2d_msgs::msg::Twist2DStamped cmd_vel;
           cmd_vel.header.stamp = clock_->now(   );
           cmd_vel.velocity = best.traj.velocity;
          
           // debrief stateful scoring functions
           for (  TrajectoryCritic::Ptr & critic : critics_ ) {
           critic->debrief(  cmd_vel.velocity );
           }
          
           lock.unlock(   );
          
           pub_->publishLocalPlan(  pose.header,   best.traj );
           pub_->publishCostGrid(  costmap_ros_,   critics_ );
          
           return cmd_vel;
           } catch (  const dwb_core::NoLegalTrajectoriesException & e ) {
           nav_2d_msgs::msg::Twist2D empty_cmd;
           dwb_msgs::msg::Trajectory2D empty_traj;
           // debrief stateful scoring functions
           for (  TrajectoryCritic::Ptr & critic : critics_ ) {
           critic->debrief(  empty_cmd );
           }
          
           lock.unlock(   );
          
           pub_->publishLocalPlan(  pose.header,   empty_traj );
           pub_->publishCostGrid(  costmap_ros_,   critics_ );
          
           throw;
           }
          }
          
          dwb_msgs::msg::TrajectoryScore
     341  DWBLocalPlanner::coreScoringAlgorithm(  
     342   const geometry_msgs::msg::Pose2D & pose,  
     343   const nav_2d_msgs::msg::Twist2D velocity,  
     344   std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results )
          {
           nav_2d_msgs::msg::Twist2D twist;
           dwb_msgs::msg::Trajectory2D traj;
           dwb_msgs::msg::TrajectoryScore best,   worst;
           best.total = -1;
           worst.total = -1;
           IllegalTrajectoryTracker tracker;
          
           traj_generator_->startNewIteration(  velocity );
           while (  traj_generator_->hasMoreTwists(   ) ) {
           twist = traj_generator_->nextTwist(   );
           traj = traj_generator_->generateTrajectory(  pose,   velocity,   twist );
          
           try {
           dwb_msgs::msg::TrajectoryScore score = scoreTrajectory(  traj,   best.total );
           tracker.addLegalTrajectory(   );
           if (  results ) {
           results->twists.push_back(  score );
           }
           if (  best.total < 0 || score.total < best.total ) {
           best = score;
           if (  results ) {
           results->best_index = results->twists.size(   ) - 1;
           }
           }
           if (  worst.total < 0 || score.total > worst.total ) {
           worst = score;
           if (  results ) {
           results->worst_index = results->twists.size(   ) - 1;
           }
           }
           } catch (  const dwb_core::IllegalTrajectoryException & e ) {
           if (  results ) {
           dwb_msgs::msg::TrajectoryScore failed_score;
           failed_score.traj = traj;
          
           dwb_msgs::msg::CriticScore cs;
           cs.name = e.getCriticName(   );
           cs.raw_score = -1.0;
           failed_score.scores.push_back(  cs );
           failed_score.total = -1.0;
           results->twists.push_back(  failed_score );
           }
           tracker.addIllegalTrajectory(  e );
           }
           }
          
           if (  best.total < 0 ) {
           if (  debug_trajectory_details_ ) {
           RCLCPP_ERROR(  rclcpp::get_logger(  "DWBLocalPlanner" ),   "%s",   tracker.getMessage(   ).c_str(   ) );
           for (  auto const & x : tracker.getPercentages(   ) ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "DWBLocalPlanner" ),   "%.2f: %10s/%s",   x.second,  
           x.first.first.c_str(   ),   x.first.second.c_str(   ) );
           }
           }
           throw NoLegalTrajectoriesException(  tracker );
           }
          
           return best;
          }
          
          dwb_msgs::msg::TrajectoryScore
     409  DWBLocalPlanner::scoreTrajectory(  
     410   const dwb_msgs::msg::Trajectory2D & traj,  
           double best_score )
          {
           dwb_msgs::msg::TrajectoryScore score;
           score.traj = traj;
          
           for (  TrajectoryCritic::Ptr & critic : critics_ ) {
           dwb_msgs::msg::CriticScore cs;
           cs.name = critic->getName(   );
           cs.scale = critic->getScale(   );
          
           if (  cs.scale == 0.0 ) {
           score.scores.push_back(  cs );
           continue;
           }
          
           double critic_score = critic->scoreTrajectory(  traj );
           cs.raw_score = critic_score;
           score.scores.push_back(  cs );
           score.total += critic_score * cs.scale;
           if (  short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score ) {
           // since we keep adding positives,   once we are worse than the best,   we will stay worse
           break;
           }
           }
          
           return score;
          }
          
          nav_2d_msgs::msg::Path2D
     440  DWBLocalPlanner::transformGlobalPlan(  
     441   const nav_2d_msgs::msg::Pose2DStamped & pose )
          {
           if (  global_plan_.poses.empty(   ) ) {
           throw nav2_core::PlannerException(  "Received plan with zero length" );
           }
          
           // let's get the pose of the robot in the frame of the plan
           nav_2d_msgs::msg::Pose2DStamped robot_pose;
           if (  !nav_2d_utils::transformPose(  
           tf_,   global_plan_.header.frame_id,   pose,  
           robot_pose,   transform_tolerance_ ) )
           {
           throw dwb_core::
           PlannerTFException(  "Unable to transform robot pose into global plan's frame" );
           }
          
           // we'll discard points on the plan that are outside the local costmap
           nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(   );
           double dist_threshold = std::max(  costmap->getSizeInCellsX(   ),   costmap->getSizeInCellsY(   ) ) *
           costmap->getResolution(   ) / 2.0;
          
          
           // If prune_plan is enabled (  it is by default ) then we want to restrict the
           // plan to distances within that range as well.
           double prune_dist = prune_distance_;
          
           // Set the maximum distance we'll include points before getting to the part
           // of the path where the robot is located (  the start of the plan ). Basically,  
           // these are the points the robot has already passed.
           double transform_start_threshold;
           if (  prune_plan_ ) {
           transform_start_threshold = std::min(  dist_threshold,   prune_dist );
           } else {
           transform_start_threshold = dist_threshold;
           }
          
           // Set the maximum distance we'll include points after the part of the part of
           // the plan near the robot (  the end of the plan ). This determines the amount
           // of the plan passed on to the critics
           double transform_end_threshold;
           if (  shorten_transformed_plan_ ) {
           transform_end_threshold = std::min(  dist_threshold,   prune_dist );
           } else {
           transform_end_threshold = dist_threshold;
           }
          
           // Find the first pose in the global plan that's further than prune distance
           // from the robot using integrated distance
           auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(  
           global_plan_.poses.begin(   ),   global_plan_.poses.end(   ),   prune_dist );
          
           // Find the first pose in the plan (  upto prune_point ) that's less than transform_start_threshold
           // from the robot.
           auto transformation_begin = std::find_if(  
           begin(  global_plan_.poses ),   prune_point,  
           [&](  const auto & global_plan_pose ) {
           return euclidean_distance(  robot_pose.pose,   global_plan_pose ) < transform_start_threshold;
           } );
          
           // Find the first pose in the end of the plan that's further than transform_end_threshold
           // from the robot using integrated distance
           auto transformation_end = std::find_if(  
           transformation_begin,   global_plan_.poses.end(   ),  
           [&](  const auto & pose ) {
           return euclidean_distance(  pose,   robot_pose.pose ) > transform_end_threshold;
           } );
          
           // Transform the near part of the global plan into the robot's frame of reference.
           nav_2d_msgs::msg::Path2D transformed_plan;
           transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID(   );
           transformed_plan.header.stamp = pose.header.stamp;
          
           // Helper function for the transform below. Converts a pose2D from global
           // frame to local
           auto transformGlobalPoseToLocal = [&](  const auto & global_plan_pose ) {
           nav_2d_msgs::msg::Pose2DStamped stamped_pose,   transformed_pose;
           stamped_pose.header.frame_id = global_plan_.header.frame_id;
           stamped_pose.pose = global_plan_pose;
           nav_2d_utils::transformPose(  
           tf_,   transformed_plan.header.frame_id,  
           stamped_pose,   transformed_pose,   transform_tolerance_ );
           return transformed_pose.pose;
           };
          
           std::transform(  
           transformation_begin,   transformation_end,  
           std::back_inserter(  transformed_plan.poses ),  
           transformGlobalPoseToLocal );
          
           // Remove the portion of the global plan that we've already passed so we don't
           // process it on the next iteration.
           if (  prune_plan_ ) {
           global_plan_.poses.erase(  begin(  global_plan_.poses ),   transformation_begin );
           pub_->publishGlobalPlan(  global_plan_ );
           }
          
           if (  transformed_plan.poses.empty(   ) ) {
           throw nav2_core::PlannerException(  "Resulting plan has 0 poses in it." );
           }
           return transformed_plan;
          }
          
          } // namespace dwb_core
          
          // Register this controller as a nav2_core plugin
     546  PLUGINLIB_EXPORT_CLASS(  
           dwb_core::DWBLocalPlanner,  
           nav2_core::Controller )

navigation2/nav2_dwb_controller/dwb_core/src/illegal_trajectory_tracker.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_core/illegal_trajectory_tracker.hpp"
          #include <map>
          #include <utility>
          #include <string>
          #include <sstream>
          
          namespace dwb_core
          {
      43  void IllegalTrajectoryTracker::addIllegalTrajectory(  
      44   const dwb_core::IllegalTrajectoryException & e )
          {
           counts_[std::make_pair(  e.getCriticName(   ),   e.what(   ) )]++;
           illegal_count_++;
          }
          
      50  void IllegalTrajectoryTracker::addLegalTrajectory(   )
          {
           legal_count_++;
          }
          
          std::map<std::pair<std::string,   std::string>,  
      56   double> IllegalTrajectoryTracker::getPercentages(   ) const
          {
           std::map<std::pair<std::string,   std::string>,   double> percents;
           double denominator = static_cast<double>(  legal_count_ + illegal_count_ );
           for (  auto const & x : counts_ ) {
           percents[x.first] = static_cast<double>(  x.second ) / denominator;
           }
           return percents;
          }
          
      66  std::string IllegalTrajectoryTracker::getMessage(   ) const
          {
           std::ostringstream msg;
           if (  legal_count_ == 0 ) {
           msg << "No valid trajectories out of " << illegal_count_ << "! ";
           } else {
           unsigned int total = legal_count_ + illegal_count_;
           msg << legal_count_ << " valid trajectories found (  ";
           msg << static_cast<double>(  100 * legal_count_ ) / static_cast<double>(  total );
           msg << "% of " << total << " ). ";
           }
           return msg.str(   );
          }
          
          } // namespace dwb_core

navigation2/nav2_dwb_controller/dwb_core/src/publisher.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_core/publisher.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <vector>
          #include <utility>
          
          #include "sensor_msgs/point_cloud2_iterator.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "sensor_msgs/msg/point_cloud2.hpp"
          #include "visualization_msgs/msg/marker_array.hpp"
          #include "visualization_msgs/msg/marker.hpp"
          
          using std::max;
          using std::string;
          using nav2_util::declare_parameter_if_not_declared;
          
          namespace dwb_core
          {
          
      57  DWBPublisher::DWBPublisher(  
      58   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      59   const std::string & plugin_name )
          : node_(  parent ),  
           plugin_name_(  plugin_name )
          {
           auto node = node_.lock(   );
           clock_ = node->get_clock(   );
          }
          
          nav2_util::CallbackReturn
      68  DWBPublisher::on_configure(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_evaluation",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_global_plan",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_transformed_plan",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_local_plan",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_trajectories",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".publish_cost_grid_pc",  
           rclcpp::ParameterValue(  false ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".marker_lifetime",  
           rclcpp::ParameterValue(  0.1 ) );
          
           node->get_parameter(  plugin_name_ + ".publish_evaluation",   publish_evaluation_ );
           node->get_parameter(  plugin_name_ + ".publish_global_plan",   publish_global_plan_ );
           node->get_parameter(  plugin_name_ + ".publish_transformed_plan",   publish_transformed_ );
           node->get_parameter(  plugin_name_ + ".publish_local_plan",   publish_local_plan_ );
           node->get_parameter(  plugin_name_ + ".publish_trajectories",   publish_trajectories_ );
           node->get_parameter(  plugin_name_ + ".publish_cost_grid_pc",   publish_cost_grid_pc_ );
          
           eval_pub_ = node->create_publisher<dwb_msgs::msg::LocalPlanEvaluation>(  "evaluation",   1 );
           global_pub_ = node->create_publisher<nav_msgs::msg::Path>(  "received_global_plan",   1 );
           transformed_pub_ = node->create_publisher<nav_msgs::msg::Path>(  "transformed_global_plan",   1 );
           local_pub_ = node->create_publisher<nav_msgs::msg::Path>(  "local_plan",   1 );
           marker_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>(  "marker",   1 );
           cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(  "cost_cloud",   1 );
          
           double marker_lifetime = 0.0;
           node->get_parameter(  plugin_name_ + ".marker_lifetime",   marker_lifetime );
           marker_lifetime_ = rclcpp::Duration::from_seconds(  marker_lifetime );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     119  DWBPublisher::on_activate(   )
          {
           eval_pub_->on_activate(   );
           global_pub_->on_activate(   );
           transformed_pub_->on_activate(   );
           local_pub_->on_activate(   );
           marker_pub_->on_activate(   );
           cost_grid_pc_pub_->on_activate(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     132  DWBPublisher::on_deactivate(   )
          {
           eval_pub_->on_deactivate(   );
           global_pub_->on_deactivate(   );
           transformed_pub_->on_deactivate(   );
           local_pub_->on_deactivate(   );
           marker_pub_->on_deactivate(   );
           cost_grid_pc_pub_->on_deactivate(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     145  DWBPublisher::on_cleanup(   )
          {
           eval_pub_.reset(   );
           global_pub_.reset(   );
           transformed_pub_.reset(   );
           local_pub_.reset(   );
           marker_pub_.reset(   );
           cost_grid_pc_pub_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          void
     158  DWBPublisher::publishEvaluation(  std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results )
          {
           if (  results ) {
           if (  publish_evaluation_ && eval_pub_->get_subscription_count(   ) > 0 ) {
           auto msg = std::make_unique<dwb_msgs::msg::LocalPlanEvaluation>(  *results );
           eval_pub_->publish(  std::move(  msg ) );
           }
           publishTrajectories(  *results );
           }
          }
          
          void
     170  DWBPublisher::publishTrajectories(  const dwb_msgs::msg::LocalPlanEvaluation & results )
          {
           if (  marker_pub_->get_subscription_count(   ) < 1 ) {return;}
          
           if (  !publish_trajectories_ ) {return;}
           auto ma = std::make_unique<visualization_msgs::msg::MarkerArray>(   );
           visualization_msgs::msg::Marker m;
          
           if (  results.twists.size(   ) == 0 ) {return;}
          
           geometry_msgs::msg::Point pt;
          
           m.header = results.header;
           m.type = m.LINE_STRIP;
           m.pose.orientation.w = 1;
           m.scale.x = 0.002;
           m.color.a = 1.0;
           m.lifetime = marker_lifetime_;
          
           double best_cost = results.twists[results.best_index].total;
           double worst_cost = results.twists[results.worst_index].total;
           double denominator = worst_cost - best_cost;
          
           if (  std::fabs(  denominator ) < 1e-9 ) {
           denominator = 1.0;
           }
          
           unsigned currentValidId = 0;
           unsigned currentInvalidId = 0;
           string validNamespace(  "ValidTrajectories" );
           string invalidNamespace(  "InvalidTrajectories" );
           for (  unsigned int i = 0; i < results.twists.size(   ); i++ ) {
           const dwb_msgs::msg::TrajectoryScore & twist = results.twists[i];
           double displayLevel = (  twist.total - best_cost ) / denominator;
           if (  twist.total >= 0 ) {
           m.color.r = displayLevel;
           m.color.g = 1.0 - displayLevel;
           m.color.b = 0;
           m.color.a = 1.0;
           m.ns = validNamespace;
           m.id = currentValidId;
           ++currentValidId;
           } else {
           m.color.r = 0;
           m.color.g = 0;
           m.color.b = 0;
           m.color.a = 1.0;
           m.ns = invalidNamespace;
           m.id = currentInvalidId;
           ++currentInvalidId;
           }
           m.points.clear(   );
           for (  unsigned int j = 0; j < twist.traj.poses.size(   ); ++j ) {
           pt.x = twist.traj.poses[j].x;
           pt.y = twist.traj.poses[j].y;
           pt.z = 0;
           m.points.push_back(  pt );
           }
           ma->markers.push_back(  m );
           }
           marker_pub_->publish(  std::move(  ma ) );
          }
          
          void
     234  DWBPublisher::publishLocalPlan(  
     235   const std_msgs::msg::Header & header,  
     236   const dwb_msgs::msg::Trajectory2D & traj )
          {
           if (  !publish_local_plan_ ) {return;}
          
           auto path =
           std::make_unique<nav_msgs::msg::Path>(  
           nav_2d_utils::poses2DToPath(  
           traj.poses,   header.frame_id,  
           header.stamp ) );
          
           if (  local_pub_->get_subscription_count(   ) > 0 ) {
           local_pub_->publish(  std::move(  path ) );
           }
          }
          
          void
     252  DWBPublisher::publishCostGrid(  
     253   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,  
     254   const std::vector<TrajectoryCritic::Ptr> critics )
          {
           if (  cost_grid_pc_pub_->get_subscription_count(   ) < 1 ) {return;}
          
           if (  !publish_cost_grid_pc_ ) {return;}
          
           auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud2>(   );
           cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID(   );
           cost_grid_pc->header.stamp = clock_->now(   );
          
           nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(   );
           double x_coord,   y_coord;
           unsigned int size_x = costmap->getSizeInCellsX(   );
           unsigned int size_y = costmap->getSizeInCellsY(   );
          
           std::vector<std::pair<std::string,   std::vector<float>>> cost_channels;
           std::vector<float> total_cost(  size_x * size_y,   0.0 );
          
           for (  TrajectoryCritic::Ptr critic : critics ) {
           unsigned int channel_index = cost_channels.size(   );
           critic->addCriticVisualization(  cost_channels );
           if (  channel_index == cost_channels.size(   ) ) {
           // No channels were added,   so skip to next critic
           continue;
           }
           double scale = critic->getScale(   );
           for (  unsigned int i = 0; i < size_x * size_y; i++ ) {
           total_cost[i] += cost_channels[channel_index].second[i] * scale;
           }
           }
          
           cost_channels.push_back(  std::make_pair(  "total_cost",   total_cost ) );
          
           cost_grid_pc->width = size_x * size_y;
           cost_grid_pc->height = 1;
           cost_grid_pc->fields.resize(  3 + cost_channels.size(   ) ); // x,  y,  z,   + cost channels
           cost_grid_pc->is_dense = true;
           cost_grid_pc->is_bigendian = false;
          
           int offset = 0;
           for (  size_t i = 0; i < cost_grid_pc->fields.size(   ); ++i,   offset += 4 ) {
           cost_grid_pc->fields[i].offset = offset;
           cost_grid_pc->fields[i].count = 1;
           cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
           if (  i >= 3 ) {
           cost_grid_pc->fields[i].name = cost_channels[i - 3].first;
           }
           }
          
           cost_grid_pc->fields[0].name = "x";
           cost_grid_pc->fields[1].name = "y";
           cost_grid_pc->fields[2].name = "z";
          
           cost_grid_pc->point_step = offset;
           cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width;
           cost_grid_pc->data.resize(  cost_grid_pc->row_step * cost_grid_pc->height );
          
           std::vector<sensor_msgs::PointCloud2Iterator<float>> cost_grid_pc_iter;
          
           for (  size_t i = 0; i < cost_grid_pc->fields.size(   ); ++i ) {
           sensor_msgs::PointCloud2Iterator<float> iter(  *cost_grid_pc,   cost_grid_pc->fields[i].name );
           cost_grid_pc_iter.push_back(  iter );
           }
          
           unsigned int j = 0;
           for (  unsigned int cy = 0; cy < size_y; cy++ ) {
           for (  unsigned int cx = 0; cx < size_x; cx++ ) {
           costmap->mapToWorld(  cx,   cy,   x_coord,   y_coord );
           *cost_grid_pc_iter[0] = x_coord;
           *cost_grid_pc_iter[1] = y_coord;
           *cost_grid_pc_iter[2] = 0.0; // z value
          
           for (  size_t i = 3; i < cost_grid_pc_iter.size(   ); ++i ) {
           *cost_grid_pc_iter[i] = cost_channels[i - 3].second[j];
           ++cost_grid_pc_iter[i];
           }
           ++cost_grid_pc_iter[0];
           ++cost_grid_pc_iter[1];
           ++cost_grid_pc_iter[2];
           j++;
           }
           }
          
           cost_grid_pc_pub_->publish(  std::move(  cost_grid_pc ) );
          }
          
          void
     341  DWBPublisher::publishGlobalPlan(  const nav_2d_msgs::msg::Path2D plan )
          {
           publishGenericPlan(  plan,   *global_pub_,   publish_global_plan_ );
          }
          
          void
     347  DWBPublisher::publishTransformedPlan(  const nav_2d_msgs::msg::Path2D plan )
          {
           publishGenericPlan(  plan,   *transformed_pub_,   publish_transformed_ );
          }
          
          void
     353  DWBPublisher::publishLocalPlan(  const nav_2d_msgs::msg::Path2D plan )
          {
           publishGenericPlan(  plan,   *local_pub_,   publish_local_plan_ );
          }
          
          void
     359  DWBPublisher::publishGenericPlan(  
     360   const nav_2d_msgs::msg::Path2D plan,  
     361   rclcpp::Publisher<nav_msgs::msg::Path> & pub,   bool flag )
          {
           if (  pub.get_subscription_count(   ) < 1 ) {return;}
           if (  !flag ) {return;}
           auto path = std::make_unique<nav_msgs::msg::Path>(  nav_2d_utils::pathToPath(  plan ) );
           pub.publish(  std::move(  path ) );
          }
          
          } // namespace dwb_core

navigation2/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <dwb_core/trajectory_utils.hpp>
          
          #include <cmath>
          
          #include "rclcpp/duration.hpp"
          
          #include "dwb_core/exceptions.hpp"
          
          namespace dwb_core
          {
      45  const geometry_msgs::msg::Pose2D & getClosestPose(  
           const dwb_msgs::msg::Trajectory2D & trajectory,  
           const double time_offset )
          {
           rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(  time_offset );
           const unsigned int num_poses = trajectory.poses.size(   );
           if (  num_poses == 0 ) {
           throw nav2_core::PlannerException(  "Cannot call getClosestPose on empty trajectory." );
           }
           unsigned int closest_index = num_poses;
           double closest_diff = 0.0;
           for (  unsigned int i = 0; i < num_poses; ++i ) {
           double diff = std::fabs(  (  rclcpp::Duration(  trajectory.time_offsets[i] ) - goal_time ).seconds(   ) );
           if (  closest_index == num_poses || diff < closest_diff ) {
           closest_index = i;
           closest_diff = diff;
           }
           if (  goal_time < rclcpp::Duration(  trajectory.time_offsets[i] ) ) {
           break;
           }
           }
           return trajectory.poses[closest_index];
          }
          
      69  geometry_msgs::msg::Pose2D projectPose(  
           const dwb_msgs::msg::Trajectory2D & trajectory,  
           const double time_offset )
          {
           rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(  time_offset );
           const unsigned int num_poses = trajectory.poses.size(   );
           if (  num_poses == 0 ) {
           throw nav2_core::PlannerException(  "Cannot call projectPose on empty trajectory." );
           }
           if (  goal_time <= (  trajectory.time_offsets[0] ) ) {
           return trajectory.poses[0];
           } else if (  goal_time >= rclcpp::Duration(  trajectory.time_offsets[num_poses - 1] ) ) {
           return trajectory.poses[num_poses - 1];
           }
          
           for (  unsigned int i = 0; i < num_poses - 1; ++i ) {
           if (  goal_time >= rclcpp::Duration(  trajectory.time_offsets[i] ) &&
           goal_time < rclcpp::Duration(  trajectory.time_offsets[i + 1] ) )
           {
           double time_diff =
           (  rclcpp::Duration(  trajectory.time_offsets[i + 1] ) -
           rclcpp::Duration(  trajectory.time_offsets[i] ) ).seconds(   );
           double ratio = (  goal_time - rclcpp::Duration(  trajectory.time_offsets[i] ) ).seconds(   ) /
           time_diff;
           double inv_ratio = 1.0 - ratio;
           const geometry_msgs::msg::Pose2D & pose_a = trajectory.poses[i];
           const geometry_msgs::msg::Pose2D & pose_b = trajectory.poses[i + 1];
           geometry_msgs::msg::Pose2D projected;
           projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
           projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
           projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
           return projected;
           }
           }
          
           // Should not reach this point
           return trajectory.poses[num_poses - 1];
          }
          
          
          } // namespace dwb_core

navigation2/nav2_dwb_controller/dwb_core/test/utils_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "gtest/gtest.h"
          #include "dwb_core/trajectory_utils.hpp"
          
          using dwb_core::getClosestPose;
          using dwb_core::projectPose;
          
      41  TEST(  Utils,   ClosestPose )
          {
           dwb_msgs::msg::Trajectory2D traj;
           traj.poses.resize(  4 );
           traj.time_offsets.resize(  4 );
           for (  unsigned int i = 0; i < traj.poses.size(   ); i++ ) {
           double d = static_cast<double>(  i );
           traj.poses[i].x = d;
           traj.time_offsets[i] = rclcpp::Duration::from_seconds(  d );
           }
          
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   0.0 ).x,   traj.poses[0].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   -1.0 ).x,   traj.poses[0].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   0.4 ).x,   traj.poses[0].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   0.5 ).x,   traj.poses[0].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   0.51 ).x,   traj.poses[1].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   1.0 ).x,   traj.poses[1].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   1.4999 ).x,   traj.poses[1].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   2.0 ).x,   traj.poses[2].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   2.51 ).x,   traj.poses[3].x );
           EXPECT_DOUBLE_EQ(  getClosestPose(  traj,   3.5 ).x,   traj.poses[3].x );
          }
          
      64  TEST(  Utils,   ProjectPose )
          {
           dwb_msgs::msg::Trajectory2D traj;
           traj.poses.resize(  4 );
           traj.time_offsets.resize(  4 );
           for (  unsigned int i = 0; i < traj.poses.size(   ); i++ ) {
           double d = static_cast<double>(  i );
           traj.poses[i].x = d;
           traj.poses[i].y = 30.0 - 2.0 * d;
           traj.poses[i].theta = 0.42;
           traj.time_offsets[i] = rclcpp::Duration::from_seconds(  d );
           }
          
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.0 ).x,   0.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.0 ).y,   30.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.0 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   -1.0 ).x,   0.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   -1.0 ).y,   30.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   -1.0 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.4 ).x,   0.4 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.4 ).y,   29.2 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.4 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.5 ).x,   0.5 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.5 ).y,   29.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.5 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.51 ).x,   0.51 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.51 ).y,   28.98 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   0.51 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.0 ).x,   1.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.0 ).y,   28.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.0 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.4999 ).x,   1.4999 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.4999 ).y,   27.0002 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   1.4999 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   2.0 ).x,   2.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   2.0 ).y,   26.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   2.0 ).theta,   0.42 );
           EXPECT_FLOAT_EQ(  projectPose(  traj,   2.51 ).x,   2.51 );
           EXPECT_FLOAT_EQ(  projectPose(  traj,   2.51 ).y,   24.98 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   2.51 ).theta,   0.42 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   3.5 ).x,   3.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   3.5 ).y,   24.0 );
           EXPECT_DOUBLE_EQ(  projectPose(  traj,   3.5 ).theta,   0.42 );
          }
          
     109  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/alignment_util.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__ALIGNMENT_UTIL_HPP_
          #define DWB_CRITICS__ALIGNMENT_UTIL_HPP_
          
          #include "geometry_msgs/msg/pose2_d.hpp"
          
          namespace dwb_critics
          {
          /**
           * @brief Projects the given pose forward the specified distance in the x direction.
           * @param pose Input pose
           * @param distance distance to move (  in meters )
           * @return Pose distance meters in front of input pose.
           *
           * (  used in both path_align and dist_align )
           */
      50  geometry_msgs::msg::Pose2D getForwardPose(  const geometry_msgs::msg::Pose2D & pose,   double distance );
          
          } // namespace dwb_critics
          
          #endif // DWB_CRITICS__ALIGNMENT_UTIL_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__BASE_OBSTACLE_HPP_
          #define DWB_CRITICS__BASE_OBSTACLE_HPP_
          
          #include <string>
          #include <vector>
          #include <utility>
          
          #include "dwb_core/trajectory_critic.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class BaseObstacleCritic
           * @brief Uses costmap 2d to assign negative costs if a circular robot would collide at any point of the trajectory.
           *
           * This class can only be used to figure out if a circular robot is in collision. If the cell corresponding
           * with any of the poses in the Trajectory is an obstacle,   inscribed obstacle or unknown,   it will return a
           * negative cost. Otherwise it will return either the final pose's cost,   or the sum of all poses,   depending
           * on the sum_scores parameter.
           *
           * Other classes (  like ObstacleFootprintCritic ) can do more advanced checking for collisions.
           */
      57  class BaseObstacleCritic : public dwb_core::TrajectoryCritic
          {
          public:
           void onInit(   ) override;
           double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
           void addCriticVisualization(  
           std::vector<std::pair<std::string,   std::vector<float>>> & cost_channels ) override;
          
           /**
           * @brief Return the obstacle score for a particular pose
           * @param pose Pose to check
           */
           virtual double scorePose(  const geometry_msgs::msg::Pose2D & pose );
          
           /**
           * @brief Check to see whether a given cell cost is valid for driving through.
           * @param cost Cost of the cell
           * @return Return true if valid cell
           */
           virtual bool isValidCost(  const unsigned char cost );
          
          protected:
           nav2_costmap_2d::Costmap2D * costmap_;
           bool sum_scores_;
          };
          } // namespace dwb_critics
          
          #endif // DWB_CRITICS__BASE_OBSTACLE_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_align.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__GOAL_ALIGN_HPP_
          #define DWB_CRITICS__GOAL_ALIGN_HPP_
          
          #include <vector>
          #include <string>
          #include "dwb_critics/goal_dist.hpp"
          
          namespace dwb_critics
          {
          
          /**
           * @class GoalAlignCritic
           * @brief Scores trajectories based on whether the robot ends up pointing toward the eventual goal
           *
           * Similar to GoalDistCritic,   this critic finds the pose from the global path farthest from the robot
           * that is still on the costmap and then evaluates how far the front of the robot is from that point.
           * This works as a proxy to calculating which way the robot should be pointing.
           */
      52  class GoalAlignCritic : public GoalDistCritic
          {
          public:
      55   GoalAlignCritic(   )
           : forward_point_distance_(  0.0 ) {}
           void onInit(   ) override;
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
           double scorePose(  const geometry_msgs::msg::Pose2D & pose ) override;
          
          protected:
           double forward_point_distance_;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__GOAL_ALIGN_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/goal_dist.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__GOAL_DIST_HPP_
          #define DWB_CRITICS__GOAL_DIST_HPP_
          
          #include <vector>
          #include "dwb_critics/map_grid.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class GoalDistCritic
           * @brief Scores trajectories based on how far along the global path they end up.
           *
           * This trajectory critic helps ensure progress along the global path. It finds the pose from the
           * global path farthest from the robot that is still on the costmap,   and aims for that point by
           * assigning the lowest cost to the cell corresponding with that farthest pose.
           */
      50  class GoalDistCritic : public MapGridCritic
          {
          public:
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
          
          protected:
           bool getLastPoseOnCostmap(  
           const nav_2d_msgs::msg::Path2D & global_plan,   unsigned int & x,  
           unsigned int & y );
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__GOAL_DIST_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/line_iterator.hpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__LINE_ITERATOR_HPP_
          #define DWB_CRITICS__LINE_ITERATOR_HPP_
          
          #include <stdlib.h>
          
          namespace dwb_critics
          {
          
          /** An iterator implementing Bresenham Ray-Tracing. */
      38  class LineIterator
          {
          public:
      41   LineIterator(  int x0,   int y0,   int x1,   int y1 )
           : x0_(  x0 ),  
           y0_(  y0 ),  
           x1_(  x1 ),  
           y1_(  y1 ),  
           x_(  x0 ),   // X and Y start of at first endpoint.
           y_(  y0 ),  
           deltax_(  abs(  x1 - x0 ) ),  
           deltay_(  abs(  y1 - y0 ) ),  
           curpixel_(  0 )
           {
           if (  x1_ >= x0_ ) { // The x-values are increasing
           xinc1_ = 1;
           xinc2_ = 1;
           } else { // The x-values are decreasing
           xinc1_ = -1;
           xinc2_ = -1;
           }
          
           if (  y1_ >= y0_ ) { // The y-values are increasing
           yinc1_ = 1;
           yinc2_ = 1;
           } else { // The y-values are decreasing
           yinc1_ = -1;
           yinc2_ = -1;
           }
          
           if (  deltax_ >= deltay_ ) { // There is at least one x-value for every y-value
           xinc1_ = 0; // Don't change the x when numerator >= denominator
           yinc2_ = 0; // Don't change the y for every iteration
           den_ = deltax_;
           num_ = deltax_ / 2;
           numadd_ = deltay_;
           numpixels_ = deltax_; // There are more x-values than y-values
           } else { // There is at least one y-value for every x-value
           xinc2_ = 0; // Don't change the x for every iteration
           yinc1_ = 0; // Don't change the y when numerator >= denominator
           den_ = deltay_;
           num_ = deltay_ / 2;
           numadd_ = deltax_;
           numpixels_ = deltay_; // There are more y-values than x-values
           }
           }
          
      85   bool isValid(   ) const
           {
           return curpixel_ <= numpixels_;
           }
          
      90   void advance(   )
           {
           num_ += numadd_; // Increase the numerator by the top of the fraction
           if (  num_ >= den_ ) { // Check if numerator >= denominator
           num_ -= den_; // Calculate the new numerator value
           x_ += xinc1_; // Change the x as appropriate
           y_ += yinc1_; // Change the y as appropriate
           }
           x_ += xinc2_; // Change the x as appropriate
           y_ += yinc2_; // Change the y as appropriate
          
           curpixel_++;
           }
          
     104   int getX(   ) const
           {
           return x_;
           }
     108   int getY(   ) const
           {
           return y_;
           }
          
     113   int getX0(   ) const
           {
           return x0_;
           }
     117   int getY0(   ) const
           {
           return y0_;
           }
          
     122   int getX1(   ) const
           {
           return x1_;
           }
     126   int getY1(   ) const
           {
           return y1_;
           }
          
          private:
           int x0_; ///< X coordinate of first end point.
           int y0_; ///< Y coordinate of first end point.
           int x1_; ///< X coordinate of second end point.
           int y1_; ///< Y coordinate of second end point.
          
           int x_; ///< X coordinate of current point.
           int y_; ///< Y coordinate of current point.
          
           int deltax_; ///< Difference between Xs of endpoints.
           int deltay_; ///< Difference between Ys of endpoints.
          
           int curpixel_; ///< index of current point in line loop.
          
           int xinc1_,   xinc2_,   yinc1_,   yinc2_;
           int den_,   num_,   numadd_,   numpixels_;
          };
          
          } // end namespace dwb_critics
          
          #endif // DWB_CRITICS__LINE_ITERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__MAP_GRID_HPP_
          #define DWB_CRITICS__MAP_GRID_HPP_
          
          #include <vector>
          #include <memory>
          #include <string>
          #include <utility>
          
          #include "dwb_core/trajectory_critic.hpp"
          #include "costmap_queue/costmap_queue.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class MapGridCritic
           * @brief breadth-first scoring of all the cells in the costmap
           *
           * This TrajectoryCritic assigns a score to every cell in the costmap based on
           * the distance to the cell from some set of source points. The cells corresponding
           * with the source points are marked with some initial score,   and then every other cell
           * is updated with a score based on its relation to the closest source cell,   based on a
           * breadth-first exploration of the cells of the costmap.
           *
           * This approach was chosen for computational efficiency,   such that each trajectory
           * need not be compared to the list of source points.
           */
      61  class MapGridCritic : public dwb_core::TrajectoryCritic
          {
          public:
           // Standard TrajectoryCritic Interface
           void onInit(   ) override;
           double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
           void addCriticVisualization(  
           std::vector<std::pair<std::string,   std::vector<float>>> & cost_channels ) override;
           double getScale(   ) const override {return costmap_->getResolution(   ) * 0.5 * scale_;}
          
           // Helper Functions
           /**
           * @brief Retrieve the score for a single pose
           * @param pose The pose to score,   assumed to be in the same frame as the costmap
           * @return The score associated with the cell of the costmap where the pose lies
           */
           virtual double scorePose(  const geometry_msgs::msg::Pose2D & pose );
          
           /**
           * @brief Retrieve the score for a particular cell of the costmap
           * @param x x-coordinate within the costmap
           * @param y y-coordinate within the costmap
           * @return the score associated with that cell.
           */
      85   inline double getScore(  unsigned int x,   unsigned int y )
           {
           return cell_values_[costmap_->getIndex(  x,   y )];
           }
          
           /**
           * @brief Sets the score of a particular cell to the obstacle cost
           * @param index Index of the cell to mark
           */
      94   void setAsObstacle(  unsigned int index );
          
          protected:
           /**
           * @brief Separate modes for aggregating scores across the multiple poses in a trajectory.
           *
           * Last returns the score associated with the last pose in the trajectory
           * Sum returns the sum of all the scores
           * Product returns the product of all the (  non-zero ) scores
           */
           // cppcheck-suppress syntaxError
     105   enum class ScoreAggregationType {Last,   Sum,   Product};
          
           /**
           * @class MapGridQueue
           * @brief Subclass of CostmapQueue that avoids Obstacles and Unknown Values
           */
     111   class MapGridQueue : public costmap_queue::CostmapQueue
           {
          public:
     114   MapGridQueue(  nav2_costmap_2d::Costmap2D & costmap,   MapGridCritic & parent )
           : costmap_queue::CostmapQueue(  costmap,   true ),   parent_(  parent ) {}
           virtual ~MapGridQueue(   ) = default;
           bool validCellToQueue(  const costmap_queue::CellData & cell ) override;
          
          protected:
           MapGridCritic & parent_;
           };
          
           /**
           * @brief Clear the queuDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore
           */
     126   void reset(   ) override;
          
           /**
           * @brief Go through the queue and set the cells to the Manhattan distance from their parents
           */
     131   void propogateManhattanDistances(   );
          
           std::shared_ptr<MapGridQueue> queue_;
           nav2_costmap_2d::Costmap2D * costmap_;
           std::vector<double> cell_values_;
           double obstacle_score_,   unreachable_score_; ///< Special cell_values
           bool stop_on_failure_;
           ScoreAggregationType aggregationType_;
          };
          } // namespace dwb_critics
          
          #endif // DWB_CRITICS__MAP_GRID_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
          #define DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
          
          #include <vector>
          #include "dwb_critics/base_obstacle.hpp"
          
          namespace dwb_critics
          {
          typedef std::vector<geometry_msgs::msg::Point> Footprint;
          
          /**
           * @brief Transform the footprint spec to be centered at the given pose
           * @param pose Robot pose
           * @param footprint_spec List of points that make up the footprint spec,   centered at 0,  0
           * @return oriented footprint
           */
      51  Footprint getOrientedFootprint(  
           const geometry_msgs::msg::Pose2D & pose,  
           const Footprint & footprint_spec );
          
          /**
           * @class ObstacleFootprintCritic
           * @brief Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory.
           *
           * Internally,   this technically only checks if the border of the footprint collides with anything for computational
           * efficiency. This is valid if the obstacles in the local costmap are inflated.
           *
           * A more robust class could check every cell within the robot's footprint without inflating the obstacles,  
           * at some computational cost. That is left as an excercise to the reader.
           */
      65  class ObstacleFootprintCritic : public BaseObstacleCritic
          {
          public:
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
           double scorePose(  const geometry_msgs::msg::Pose2D & pose ) override;
           virtual double scorePose(  
           const geometry_msgs::msg::Pose2D & pose,  
           const Footprint & oriented_footprint );
           double getScale(   ) const override {return costmap_->getResolution(   ) * scale_;}
          
          protected:
           /**
           * @brief Rasterizes a line in the costmap grid and checks for collisions
           * @param x0 The x position of the first cell in grid coordinates
           * @param y0 The y position of the first cell in grid coordinates
           * @param x1 The x position of the second cell in grid coordinates
           * @param y1 The y position of the second cell in grid coordinates
           * @return A positive cost for a legal line... negative otherwise
           */
           double lineCost(  int x0,   int x1,   int y0,   int y1 );
          
           /**
           * @brief Checks the cost of a point in the costmap
           * @param x The x position of the point in cell coordinates
           * @param y The y position of the point in cell coordinates
           * @return A positive cost for a legal point... negative otherwise
           */
      94   double pointCost(  int x,   int y );
          
           Footprint footprint_spec_;
          };
          } // namespace dwb_critics
          
          #endif // DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__OSCILLATION_HPP_
          #define DWB_CRITICS__OSCILLATION_HPP_
          
          #include <vector>
          #include <string>
          #include <chrono>
          #include "dwb_core/trajectory_critic.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          
          namespace dwb_critics
          {
          
          /**
           * @class OscillationCritic
           * @brief Checks to see whether the sign of the commanded velocity flips frequently
           *
           * This critic figures out if the commanded trajectories are oscillating by seeing
           * if one of the dimensions (  x,  y,  theta ) flips from positive to negative and then back
           * (  or vice versa ) without moving sufficiently far or waiting sufficiently long.
           *
           * Scenario 1: Robot moves one meter forward,   and then two millimeters backward.
           * Another forward motion would be considered oscillating,   since the x dimension would then
           * flip from positive to negative and then back to negative. Hence,   when scoring different
           * trajectories,   positive velocity commands will get the oscillation_score (  -5.0,   or invalid )
           * and only negative velocity commands will be considered valid.
          
           * Scenario 2: Robot moves one meter forward,   and then one meter backward.
           * The robot has thus moved one meter since flipping the sign of the x direction,   which
           * is greater than our oscillation_reset_dist,   so its not considered oscillating,   so all
           * trajectories are considered valid.
           *
           * Note: The critic will only check oscillations in the x dimension while it exceeds
           * a particular value (  x_only_threshold_ ). If it dips below that magnitude,   it will
           * also check for oscillations in the y and theta dimensions. If x_only_threshold_ is
           * negative,   then the critic will always check all dimensions.
           *
           * Implementation Details:
           * The critic saves the robot's current position when it prepares,   and what the actual
           * commanded velocity was during the debrief step. Upon debriefing,   if the sign of any of
           * dimensions has flipped since the last command,   the position is saved as prev_stationary_pose_.
           *
           * If the linear or angular distance from prev_stationary_pose_ to the current pose exceeds
           * the limits,   the oscillation flags are reset so the previous sign change is no longer remembered.
           * This assumes that oscillation_reset_dist_ or oscillation_reset_angle_ are positive. Otherwise,  
           * it uses a time based delay reset function.
           */
      82  class OscillationCritic : public dwb_core::TrajectoryCritic
          {
          public:
           OscillationCritic(   )
           : oscillation_reset_time_(  0s ) {}
           void onInit(   ) override;
      88   bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
      91   double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
      92   void reset(   ) override;
      93   void debrief(  const nav_2d_msgs::msg::Twist2D & cmd_vel ) override;
          
          private:
           /**
           * @class CommandTrend
           * @brief Helper class for performing the same logic on the x,  y and theta dimensions
           */
     100   class CommandTrend
           {
          public:
     103   CommandTrend(   );
     104   void reset(   );
          
           /**
           * @brief update internal flags based on the commanded velocity
           * @param velocity commanded velocity for the dimension this trend is tracking
           * @return true if the sign has flipped
           */
     111   bool update(  double velocity );
          
           /**
           * @brief Check to see whether the proposed velocity would be considered oscillating
           * @param velocity the velocity to evaluate
           * @return true if the sign has flipped more than once
           */
     118   bool isOscillating(  double velocity );
          
           /**
           * @brief Check whether we are currently tracking a flipped sign
           * @return True if the sign has flipped
           */
     124   bool hasSignFlipped(   );
          
          private:
           // Simple Enum for Tracking
           // cppcheck-suppress syntaxError
           enum class Sign { ZERO,   POSITIVE,   NEGATIVE };
          
           Sign sign_;
           bool positive_only_,   negative_only_;
           };
          
           /**
           * @brief Given a command that has been selected,   track each component's sign for oscillations
           * @param cmd_vel The command velocity selected by the algorithm
           * @return True if the sign on any of the components flipped
           */
           bool setOscillationFlags(  const nav_2d_msgs::msg::Twist2D & cmd_vel );
          
           /**
           * @brief Return true if the robot has travelled far enough or waited long enough
           */
           bool resetAvailable(   );
          
           CommandTrend x_trend_,   y_trend_,   theta_trend_;
           double oscillation_reset_dist_,   oscillation_reset_angle_,   x_only_threshold_;
           rclcpp::Duration oscillation_reset_time_;
          
           // Cached square parameter
           double oscillation_reset_dist_sq_;
          
           // Saved positions
           geometry_msgs::msg::Pose2D pose_,   prev_stationary_pose_;
           // Saved timestamp
           rclcpp::Time prev_reset_time_;
           rclcpp::Clock::SharedPtr clock_;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__OSCILLATION_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_align.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__PATH_ALIGN_HPP_
          #define DWB_CRITICS__PATH_ALIGN_HPP_
          
          #include <vector>
          #include <string>
          #include "dwb_critics/path_dist.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class PathAlignCritic
           * @brief Scores trajectories based on how far from the global path the front of the robot ends up.
           *
           * This uses the costmap grid as a proxy for calculating which way the robot should be facing relative
           * to the global path. Instead of scoring how far the center of the robot is away from the global path,  
           * this critic calculates how far a point forward_point_distance in front of the robot is from the global
           * path. This biases the planner toward trajectories that line up with the global plan.
           *
           * When the robot is near the end of the path,   the scale of this critic is set to zero. When the projected
           * point is past the global goal,   we no longer want this critic to try to align to a part of the global path
           * that isn't there.
           */
      56  class PathAlignCritic : public PathDistCritic
          {
          public:
      59   PathAlignCritic(   )
           : zero_scale_(  false ),   forward_point_distance_(  0.0 ) {}
           void onInit(   ) override;
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
           double getScale(   ) const override;
           double scorePose(  const geometry_msgs::msg::Pose2D & pose ) override;
          
          protected:
           bool zero_scale_;
           double forward_point_distance_;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__PATH_ALIGN_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/path_dist.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__PATH_DIST_HPP_
          #define DWB_CRITICS__PATH_DIST_HPP_
          
          #include "dwb_critics/map_grid.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class PathDistCritic
           * @brief Scores trajectories based on how far from the global path they end up.
           */
      45  class PathDistCritic : public MapGridCritic
          {
          public:
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__PATH_DIST_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/prefer_forward.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__PREFER_FORWARD_HPP_
          #define DWB_CRITICS__PREFER_FORWARD_HPP_
          
          #include <string>
          #include "dwb_core/trajectory_critic.hpp"
          
          namespace dwb_critics
          {
          
          /**
           * @class PreferForwardCritic
           * @brief Penalize trajectories with move backwards and/or turn too much
           *
           * Has three different scoring conditions:
           * 1 ) If the trajectory's x velocity is negative,   return the penalty
           * 2 ) If the trajectory's x is low and the theta is also low,   return the penalty.
           * 3 ) Otherwise,   return a scaled version of the trajectory's theta.
           */
      53  class PreferForwardCritic : public dwb_core::TrajectoryCritic
          {
          public:
      56   PreferForwardCritic(   )
           : penalty_(  1.0 ),   strafe_x_(  0.1 ),   strafe_theta_(  0.2 ),   theta_scale_(  10.0 ) {}
           void onInit(   ) override;
           double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
          
          private:
           double penalty_,   strafe_x_,   strafe_theta_,   theta_scale_;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__PREFER_FORWARD_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef DWB_CRITICS__ROTATE_TO_GOAL_HPP_
          #define DWB_CRITICS__ROTATE_TO_GOAL_HPP_
          
          #include <string>
          #include <vector>
          #include "dwb_core/trajectory_critic.hpp"
          
          namespace dwb_critics
          {
          
          /**
           * @class RotateToGoalCritic
           * @brief Forces the commanded trajectories to only be rotations if within a certain distance window
           *
           * This used to be built in to the DWA Local Planner as the LatchedStopRotate controller,  
           * but has been moved to a critic for consistency.
           *
           * The critic has three distinct phases.
           * 1 ) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose,   this critic
           * will just return score 0.0.
           * 2 ) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion,   this critic
           * will only allow trajectories that are slower than the current speed in order to stop the robot (  within
           * the robot's acceleration limits ). The returned score will be the robot's linear speed squared,   multiplied
           * by the slowing_factor parameter (  default 5.0 ) added to the result of scoreRotation.
           * 3 ) If within the xy_goal_tolerance and the robot has sufficiently small linear motion,   this critic will
           * score trajectories that have linear movement as invalid and score the rest based on the result of the
           * scoreRotation method
           *
           * The scoreRotation method can be overriden,   but the default behavior is to return the shortest angular distance
           * between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.
           * * If the lookahead_time parameter is negative,   the pose evaluated will be the last pose in the trajectory,  
           * which is the same as DWA's behavior. This is the default.
           * * Otherwise,   a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead
           * time shorter than sim_time,   the critic will be less concerned about overshooting the goal yaw and thus will
           * continue to turn faster for longer.
           */
      70  class RotateToGoalCritic : public dwb_core::TrajectoryCritic
          {
          public:
           void onInit(   ) override;
           void reset(   ) override;
           bool prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,   const nav_2d_msgs::msg::Path2D & global_plan ) override;
           double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
           /**
           * @brief Assuming that this is an actual rotation when near the goal,   score the trajectory.
           *
           * This (  easily overridden ) method assumes that the critic is in the third phase (  as described above )
           * and returns a numeric score for the trajectory relative to the goal yaw.
           * @param traj Trajectory to score
           * @return numeric score
           */
           virtual double scoreRotation(  const dwb_msgs::msg::Trajectory2D & traj );
          
          private:
           bool in_window_;
           bool rotating_;
           double goal_yaw_;
           double xy_goal_tolerance_;
           double xy_goal_tolerance_sq_; ///< Cached squared tolerance
           double current_xy_speed_sq_,   stopped_xy_velocity_sq_;
           double slowing_factor_;
           double lookahead_time_;
          };
          
          } // namespace dwb_critics
          #endif // DWB_CRITICS__ROTATE_TO_GOAL_HPP_

navigation2/nav2_dwb_controller/dwb_critics/include/dwb_critics/twirling.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_CRITICS__TWIRLING_HPP_
          #define DWB_CRITICS__TWIRLING_HPP_
          
          #include "dwb_core/trajectory_critic.hpp"
          
          namespace dwb_critics
          {
          /**
           * @class TwirlingCritic
           * @brief Penalize trajectories with rotational velocities
           *
           * This class provides a cost based on how much a robot "twirls" on its way to the goal. With
           * differential-drive robots,   there isn't a choice,   but with holonomic or near-holonomic robots,  
           * sometimes a robot spins more than you'd like on its way to a goal. This class provides a way
           * to assign a penalty purely to rotational velocities.
           */
      51  class TwirlingCritic : public dwb_core::TrajectoryCritic
          {
          public:
           void onInit(   ) override;
           double scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj ) override;
          };
          } // namespace dwb_critics
          
          #endif // DWB_CRITICS__TWIRLING_HPP_

navigation2/nav2_dwb_controller/dwb_critics/src/alignment_util.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/alignment_util.hpp"
          #include <cmath>
          
          using std::cos;
          using std::sin;
          
          namespace dwb_critics
          {
      43  geometry_msgs::msg::Pose2D getForwardPose(  const geometry_msgs::msg::Pose2D & pose,   double distance )
          {
           geometry_msgs::msg::Pose2D forward_pose;
           forward_pose.x = pose.x + distance * cos(  pose.theta );
           forward_pose.y = pose.y + distance * sin(  pose.theta );
           forward_pose.theta = pose.theta;
           return forward_pose;
          }
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #include <vector>
          #include <string>
          #include <utility>
          
          #include "dwb_critics/base_obstacle.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_util/node_utils.hpp"
          
      44  PLUGINLIB_EXPORT_CLASS(  dwb_critics::BaseObstacleCritic,   dwb_core::TrajectoryCritic )
          
          namespace dwb_critics
          {
          
          void BaseObstacleCritic::onInit(   )
          {
           costmap_ = costmap_ros_->getCostmap(   );
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".sum_scores",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".sum_scores",   sum_scores_ );
          }
          
          double BaseObstacleCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           double score = 0.0;
           for (  unsigned int i = 0; i < traj.poses.size(   ); ++i ) {
           double pose_score = scorePose(  traj.poses[i] );
           // Optimized/branchless version of if (  sum_scores_ ) score += pose_score,  
           // else score = pose_score;
           score = static_cast<double>(  sum_scores_ ) * score + pose_score;
           }
           return score;
          }
          
          double BaseObstacleCritic::scorePose(  const geometry_msgs::msg::Pose2D & pose )
          {
           unsigned int cell_x,   cell_y;
           if (  !costmap_->worldToMap(  pose.x,   pose.y,   cell_x,   cell_y ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Goes Off Grid." );
           }
           unsigned char cost = costmap_->getCost(  cell_x,   cell_y );
           if (  !isValidCost(  cost ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Hits Obstacle." );
           }
           return cost;
          }
          
          bool BaseObstacleCritic::isValidCost(  const unsigned char cost )
          {
           return cost != nav2_costmap_2d::LETHAL_OBSTACLE &&
           cost != nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
           cost != nav2_costmap_2d::NO_INFORMATION;
          }
          
          void BaseObstacleCritic::addCriticVisualization(  
           std::vector<std::pair<std::string,   std::vector<float>>> & cost_channels )
          {
           std::pair<std::string,   std::vector<float>> grid_scores;
           grid_scores.first = name_;
          
           unsigned int size_x = costmap_->getSizeInCellsX(   );
           unsigned int size_y = costmap_->getSizeInCellsY(   );
           grid_scores.second.resize(  size_x * size_y );
           unsigned int i = 0;
           for (  unsigned int cy = 0; cy < size_y; cy++ ) {
           for (  unsigned int cx = 0; cx < size_x; cx++ ) {
           grid_scores.second[i] = costmap_->getCost(  cx,   cy );
           i++;
           }
           }
           cost_channels.push_back(  grid_scores );
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/goal_align.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/goal_align.hpp"
          #include <vector>
          #include <string>
          #include "dwb_critics/alignment_util.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav_2d_utils/parameters.hpp"
          
          namespace dwb_critics
          {
          
      45  void GoalAlignCritic::onInit(   )
          {
           GoalDistCritic::onInit(   );
           stop_on_failure_ = false;
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           forward_point_distance_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".forward_point_distance",   0.325 );
          }
          
      60  bool GoalAlignCritic::prepare(  
      61   const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
      62   const geometry_msgs::msg::Pose2D & goal,  
      63   const nav_2d_msgs::msg::Path2D & global_plan )
          {
           // we want the robot nose to be drawn to its final position
           // (  before robot turns towards goal orientation ),   not the end of the
           // path for the robot center. Choosing the final position after
           // turning towards goal orientation causes instability when the
           // robot needs to make a 180 degree turn at the end
           double angle_to_goal = atan2(  goal.y - pose.y,   goal.x - pose.x );
          
           nav_2d_msgs::msg::Path2D target_poses = global_plan;
           target_poses.poses.back(   ).x += forward_point_distance_ * cos(  angle_to_goal );
           target_poses.poses.back(   ).y += forward_point_distance_ * sin(  angle_to_goal );
          
           return GoalDistCritic::prepare(  pose,   vel,   goal,   target_poses );
          }
          
      79  double GoalAlignCritic::scorePose(  const geometry_msgs::msg::Pose2D & pose )
          {
           return GoalDistCritic::scorePose(  getForwardPose(  pose,   forward_point_distance_ ) );
          }
          
          } // namespace dwb_critics
          
      86  PLUGINLIB_EXPORT_CLASS(  dwb_critics::GoalAlignCritic,   dwb_core::TrajectoryCritic )

navigation2/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/goal_dist.hpp"
          #include <vector>
          #include "pluginlib/class_list_macros.hpp"
          #include "nav_2d_utils/path_ops.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
          namespace dwb_critics
          {
      43  bool GoalDistCritic::prepare(  
      44   const geometry_msgs::msg::Pose2D &,   const nav_2d_msgs::msg::Twist2D &,  
      45   const geometry_msgs::msg::Pose2D &,  
      46   const nav_2d_msgs::msg::Path2D & global_plan )
          {
           reset(   );
          
           unsigned int local_goal_x,   local_goal_y;
           if (  !getLastPoseOnCostmap(  global_plan,   local_goal_x,   local_goal_y ) ) {
           return false;
           }
          
           // Enqueue just the last pose
           int index = costmap_->getIndex(  local_goal_x,   local_goal_y );
           cell_values_[index] = 0.0;
           queue_->enqueueCell(  local_goal_x,   local_goal_y );
          
           propogateManhattanDistances(   );
          
           return true;
          }
          
      65  bool GoalDistCritic::getLastPoseOnCostmap(  
      66   const nav_2d_msgs::msg::Path2D & global_plan,  
           unsigned int & x,   unsigned int & y )
          {
           nav_2d_msgs::msg::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(  
           global_plan,  
           costmap_->getResolution(   ) );
           bool started_path = false;
          
           // skip global path points until we reach the border of the local map
           for (  unsigned int i = 0; i < adjusted_global_plan.poses.size(   ); ++i ) {
           double g_x = adjusted_global_plan.poses[i].x;
           double g_y = adjusted_global_plan.poses[i].y;
           unsigned int map_x,   map_y;
           if (  costmap_->worldToMap(  
           g_x,   g_y,   map_x,  
           map_y ) && costmap_->getCost(  map_x,   map_y ) != nav2_costmap_2d::NO_INFORMATION )
           {
           // Still on the costmap. Continue.
           x = map_x;
           y = map_y;
           started_path = true;
           } else if (  started_path ) {
           // Off the costmap after being on the costmap. Return the last saved indices.
           return true;
           }
           // else,   we have not yet found a point on the costmap,   so we just continue
           }
          
           if (  started_path ) {
           return true;
           } else {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "GoalDistCritic" ),   "None of the points of the global plan were in the local costmap." );
           return false;
           }
          }
          
          } // namespace dwb_critics
          
     106  PLUGINLIB_EXPORT_CLASS(  dwb_critics::GoalDistCritic,   dwb_core::TrajectoryCritic )

navigation2/nav2_dwb_controller/dwb_critics/src/map_grid.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/map_grid.hpp"
          #include <cmath>
          #include <string>
          #include <vector>
          #include <utility>
          #include <algorithm>
          #include <memory>
          #include "dwb_core/exceptions.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_util/node_utils.hpp"
          
          using std::abs;
          using costmap_queue::CellData;
          
          namespace dwb_critics
          {
          
          // Customization of the CostmapQueue validCellToQueue method
      53  bool MapGridCritic::MapGridQueue::validCellToQueue(  const costmap_queue::CellData & /*cell*/ )
          {
           return true;
          }
          
      58  void MapGridCritic::onInit(   )
          {
           costmap_ = costmap_ros_->getCostmap(   );
           queue_ = std::make_shared<MapGridQueue>(  *costmap_,   *this );
          
           // Always set to true,   but can be overriden by subclasses
           stop_on_failure_ = true;
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".aggregation_type",  
           rclcpp::ParameterValue(  std::string(  "last" ) ) );
          
           std::string aggro_str;
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".aggregation_type",   aggro_str );
           std::transform(  aggro_str.begin(   ),   aggro_str.end(   ),   aggro_str.begin(   ),   ::tolower );
           if (  aggro_str == "last" ) {
           aggregationType_ = ScoreAggregationType::Last;
           } else if (  aggro_str == "sum" ) {
           aggregationType_ = ScoreAggregationType::Sum;
           } else if (  aggro_str == "product" ) {
           aggregationType_ = ScoreAggregationType::Product;
           } else {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  
           "MapGridCritic" ),   "aggregation_type parameter \"%s\" invalid. Using Last.",  
           aggro_str.c_str(   ) );
           aggregationType_ = ScoreAggregationType::Last;
           }
          }
          
      94  void MapGridCritic::setAsObstacle(  unsigned int index )
          {
           cell_values_[index] = obstacle_score_;
          }
          
      99  void MapGridCritic::reset(   )
          {
           queue_->reset(   );
           cell_values_.resize(  costmap_->getSizeInCellsX(   ) * costmap_->getSizeInCellsY(   ) );
           obstacle_score_ = static_cast<double>(  cell_values_.size(   ) );
           unreachable_score_ = obstacle_score_ + 1.0;
           std::fill(  cell_values_.begin(   ),   cell_values_.end(   ),   unreachable_score_ );
          }
          
     108  void MapGridCritic::propogateManhattanDistances(   )
          {
           while (  !queue_->isEmpty(   ) ) {
           costmap_queue::CellData cell = queue_->getNextCell(   );
           cell_values_[cell.index_] = CellData::absolute_difference(  cell.src_x_,   cell.x_ ) +
           CellData::absolute_difference(  cell.src_y_,   cell.y_ );
           }
          }
          
     117  double MapGridCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           double score = 0.0;
           unsigned int start_index = 0;
           if (  aggregationType_ == ScoreAggregationType::Product ) {
           score = 1.0;
           } else if (  aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_ ) {
           start_index = traj.poses.size(   ) - 1;
           }
           double grid_dist;
          
           for (  unsigned int i = start_index; i < traj.poses.size(   ); ++i ) {
           grid_dist = scorePose(  traj.poses[i] );
           if (  stop_on_failure_ ) {
           if (  grid_dist == obstacle_score_ ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Hits Obstacle." );
           } else if (  grid_dist == unreachable_score_ ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Hits Unreachable Area." );
           }
           }
          
           switch (  aggregationType_ ) {
           case ScoreAggregationType::Last:
           score = grid_dist;
           break;
           case ScoreAggregationType::Sum:
           score += grid_dist;
           break;
           case ScoreAggregationType::Product:
           if (  score > 0 ) {
           score *= grid_dist;
           }
           break;
           }
           }
          
           return score;
          }
          
     158  double MapGridCritic::scorePose(  const geometry_msgs::msg::Pose2D & pose )
          {
           unsigned int cell_x,   cell_y;
           // we won't allow trajectories that go off the map... shouldn't happen that often anyways
           if (  !costmap_->worldToMap(  pose.x,   pose.y,   cell_x,   cell_y ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Goes Off Grid." );
           }
           return getScore(  cell_x,   cell_y );
          }
          
     169  void MapGridCritic::addCriticVisualization(  
           std::vector<std::pair<std::string,   std::vector<float>>> & cost_channels )
          {
           std::pair<std::string,   std::vector<float>> grid_scores;
           grid_scores.first = name_;
          
           nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(   );
           unsigned int size_x = costmap->getSizeInCellsX(   );
           unsigned int size_y = costmap->getSizeInCellsY(   );
           grid_scores.second.resize(  size_x * size_y );
           unsigned int i = 0;
           for (  unsigned int cy = 0; cy < size_y; cy++ ) {
           for (  unsigned int cx = 0; cx < size_x; cx++ ) {
           grid_scores.second[i] = getScore(  cx,   cy );
           i++;
           }
           }
           cost_channels.push_back(  grid_scores );
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/obstacle_footprint.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/obstacle_footprint.hpp"
          #include <algorithm>
          #include <vector>
          #include "dwb_critics/line_iterator.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
      43  PLUGINLIB_EXPORT_CLASS(  dwb_critics::ObstacleFootprintCritic,   dwb_core::TrajectoryCritic )
          
          namespace dwb_critics
          {
          Footprint getOrientedFootprint(  
           const geometry_msgs::msg::Pose2D & pose,  
           const Footprint & footprint_spec )
          {
           std::vector<geometry_msgs::msg::Point> oriented_footprint;
           oriented_footprint.resize(  footprint_spec.size(   ) );
           double cos_th = cos(  pose.theta );
           double sin_th = sin(  pose.theta );
           for (  unsigned int i = 0; i < footprint_spec.size(   ); ++i ) {
           geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
           new_pt.x = pose.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
           new_pt.y = pose.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
           }
           return oriented_footprint;
          }
          
          bool ObstacleFootprintCritic::prepare(  
           const geometry_msgs::msg::Pose2D &,   const nav_2d_msgs::msg::Twist2D &,  
           const geometry_msgs::msg::Pose2D &,   const nav_2d_msgs::msg::Path2D & )
          {
           footprint_spec_ = costmap_ros_->getRobotFootprint(   );
           if (  footprint_spec_.size(   ) == 0 ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "ObstacleFootprintCritic" ),  
           "Footprint spec is empty,   maybe missing call to setFootprint?" );
           return false;
           }
           return true;
          }
          
          double ObstacleFootprintCritic::scorePose(  const geometry_msgs::msg::Pose2D & pose )
          {
           unsigned int cell_x,   cell_y;
           if (  !costmap_->worldToMap(  pose.x,   pose.y,   cell_x,   cell_y ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Goes Off Grid." );
           }
           return scorePose(  pose,   getOrientedFootprint(  pose,   footprint_spec_ ) );
          }
          
          double ObstacleFootprintCritic::scorePose(  
           const geometry_msgs::msg::Pose2D &,  
           const Footprint & footprint )
          {
           // now we really have to lay down the footprint in the costmap grid
           unsigned int x0,   x1,   y0,   y1;
           double line_cost = 0.0;
           double footprint_cost = 0.0;
          
           // we need to rasterize each line in the footprint
           for (  unsigned int i = 0; i < footprint.size(   ) - 1; ++i ) {
           // get the cell coord of the first point
           if (  !costmap_->worldToMap(  footprint[i].x,   footprint[i].y,   x0,   y0 ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Footprint Goes Off Grid." );
           }
          
           // get the cell coord of the second point
           if (  !costmap_->worldToMap(  footprint[i + 1].x,   footprint[i + 1].y,   x1,   y1 ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Footprint Goes Off Grid." );
           }
          
           line_cost = lineCost(  x0,   x1,   y0,   y1 );
           footprint_cost = std::max(  line_cost,   footprint_cost );
           }
          
           // we also need to connect the first point in the footprint to the last point
           // get the cell coord of the last point
           if (  !costmap_->worldToMap(  footprint.back(   ).x,   footprint.back(   ).y,   x0,   y0 ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Footprint Goes Off Grid." );
           }
          
           // get the cell coord of the first point
           if (  !costmap_->worldToMap(  footprint.front(   ).x,   footprint.front(   ).y,   x1,   y1 ) ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Footprint Goes Off Grid." );
           }
          
           line_cost = lineCost(  x0,   x1,   y0,   y1 );
           footprint_cost = std::max(  line_cost,   footprint_cost );
          
           // if all line costs are legal... then we can return that the footprint is legal
           return footprint_cost;
          }
          
          double ObstacleFootprintCritic::lineCost(  int x0,   int x1,   int y0,   int y1 )
          {
           double line_cost = 0.0;
           double point_cost = -1.0;
          
           for (  LineIterator line(  x0,   y0,   x1,   y1 ); line.isValid(   ); line.advance(   ) ) {
           point_cost = pointCost(  line.getX(   ),   line.getY(   ) ); // Score the current point
          
           if (  line_cost < point_cost ) {
           line_cost = point_cost;
           }
           }
          
           return line_cost;
          }
          
          double ObstacleFootprintCritic::pointCost(  int x,   int y )
          {
           unsigned char cost = costmap_->getCost(  x,   y );
           // if the cell is in an obstacle the path is invalid or unknown
           if (  cost == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Hits Obstacle." );
           } else if (  cost == nav2_costmap_2d::NO_INFORMATION ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory Hits Unknown Region." );
           }
          
           return cost;
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/oscillation.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/oscillation.hpp"
          #include <chrono>
          #include <cmath>
          #include <string>
          #include <vector>
          #include "nav_2d_utils/parameters.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
      45  PLUGINLIB_EXPORT_CLASS(  dwb_critics::OscillationCritic,   dwb_core::TrajectoryCritic )
          
          namespace dwb_critics
          {
          
          
          OscillationCritic::CommandTrend::CommandTrend(   )
          {
           reset(   );
          }
          
          void OscillationCritic::CommandTrend::reset(   )
          {
           sign_ = Sign::ZERO;
           positive_only_ = false;
           negative_only_ = false;
          }
          
          bool OscillationCritic::CommandTrend::update(  double velocity )
          {
           bool flag_set = false;
           if (  velocity < 0.0 ) {
           if (  sign_ == Sign::POSITIVE ) {
           negative_only_ = true;
           flag_set = true;
           }
           sign_ = Sign::NEGATIVE;
           } else if (  velocity > 0.0 ) {
           if (  sign_ == Sign::NEGATIVE ) {
           positive_only_ = true;
           flag_set = true;
           }
           sign_ = Sign::POSITIVE;
           }
           return flag_set;
          }
          
          bool OscillationCritic::CommandTrend::isOscillating(  double velocity )
          {
           return (  positive_only_ && velocity < 0.0 ) || (  negative_only_ && velocity > 0.0 );
          }
          
          bool OscillationCritic::CommandTrend::hasSignFlipped(   )
          {
           return positive_only_ || negative_only_;
          }
          
          void OscillationCritic::onInit(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           clock_ = node->get_clock(   );
          
           oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist",   0.05 );
           oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
           oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle",   0.2 );
           oscillation_reset_time_ = rclcpp::Duration::from_seconds(  
           nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time",   -1.0 ) );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".x_only_threshold",   rclcpp::ParameterValue(  0.05 ) );
          
           /**
           * Historical Parameter Loading
           * If x_only_threshold is set,   use that.
           * If min_speed_xy is set in the namespace (  as it is often used for trajectory generation ),   use that.
           * If min_trans_vel is set in the namespace,   as it used to be used for trajectory generation,   complain then use that.
           * Otherwise,   set x_only_threshold_ to 0.05
           */
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".x_only_threshold",   x_only_threshold_ );
           // TODO(  crdelsey ): How to handle searchParam?
           // std::string resolved_name;
           // if (  node->hasParam(  "x_only_threshold" ) )
           // {
           // node->param(  "x_only_threshold",   x_only_threshold_ );
           // }
           // else if (  node->searchParam(  "min_speed_xy",   resolved_name ) )
           // {
           // node->param(  resolved_name,   x_only_threshold_ );
           // }
           // else if (  node->searchParam(  "min_trans_vel",   resolved_name ) )
           // {
           // ROS_WARN_NAMED(  "OscillationCritic",  
           // "Parameter min_trans_vel is deprecated. "
           // "Please use the name min_speed_xy or x_only_threshold instead." );
           // node->param(  resolved_name,   x_only_threshold_ );
           // }
           // else
           // {
           // x_only_threshold_ = 0.05;
           // }
          
           reset(   );
          }
          
          bool OscillationCritic::prepare(  
           const geometry_msgs::msg::Pose2D & pose,  
           const nav_2d_msgs::msg::Twist2D &,  
           const geometry_msgs::msg::Pose2D &,  
           const nav_2d_msgs::msg::Path2D & )
          {
           pose_ = pose;
           return true;
          }
          
          void OscillationCritic::debrief(  const nav_2d_msgs::msg::Twist2D & cmd_vel )
          {
           if (  setOscillationFlags(  cmd_vel ) ) {
           prev_stationary_pose_ = pose_;
           prev_reset_time_ = clock_->now(   );
           }
          
           // if we've got restrictions... check if we can reset any oscillation flags
           if (  x_trend_.hasSignFlipped(   ) || y_trend_.hasSignFlipped(   ) || theta_trend_.hasSignFlipped(   ) ) {
           // Reset flags if enough time or distance has passed
           if (  resetAvailable(   ) ) {
           reset(   );
           }
           }
          }
          
          bool OscillationCritic::resetAvailable(   )
          {
           if (  oscillation_reset_dist_ >= 0.0 ) {
           double x_diff = pose_.x - prev_stationary_pose_.x;
           double y_diff = pose_.y - prev_stationary_pose_.y;
           double sq_dist = x_diff * x_diff + y_diff * y_diff;
           if (  sq_dist > oscillation_reset_dist_sq_ ) {
           return true;
           }
           }
           if (  oscillation_reset_angle_ >= 0.0 ) {
           double th_diff = pose_.theta - prev_stationary_pose_.theta;
           if (  fabs(  th_diff ) > oscillation_reset_angle_ ) {
           return true;
           }
           }
           if (  oscillation_reset_time_ >= rclcpp::Duration::from_seconds(  0.0 ) ) {
           auto t_diff = (  clock_->now(   ) - prev_reset_time_ );
           if (  t_diff > oscillation_reset_time_ ) {
           return true;
           }
           }
           return false;
          }
          
          void OscillationCritic::reset(   )
          {
           x_trend_.reset(   );
           y_trend_.reset(   );
           theta_trend_.reset(   );
          }
          
          bool OscillationCritic::setOscillationFlags(  const nav_2d_msgs::msg::Twist2D & cmd_vel )
          {
           bool flag_set = false;
           // set oscillation flags for moving forward and backward
           flag_set |= x_trend_.update(  cmd_vel.x );
          
           // we'll only set flags for strafing and rotating when we're not moving forward at all
           if (  x_only_threshold_ < 0.0 || fabs(  cmd_vel.x ) <= x_only_threshold_ ) {
           flag_set |= y_trend_.update(  cmd_vel.y );
           flag_set |= theta_trend_.update(  cmd_vel.theta );
           }
           return flag_set;
          }
          
          double OscillationCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           if (  x_trend_.isOscillating(  traj.velocity.x ) ||
           y_trend_.isOscillating(  traj.velocity.y ) ||
           theta_trend_.isOscillating(  traj.velocity.theta ) )
           {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Trajectory is oscillating." );
           }
           return 0.0;
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/path_align.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/path_align.hpp"
          #include <vector>
          #include <string>
          #include "dwb_critics/alignment_util.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav_2d_utils/parameters.hpp"
          
          namespace dwb_critics
          {
          
      45  void PathAlignCritic::onInit(   )
          {
           PathDistCritic::onInit(   );
           stop_on_failure_ = false;
          
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           forward_point_distance_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".forward_point_distance",   0.325 );
          }
          
      60  bool PathAlignCritic::prepare(  
      61   const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
      62   const geometry_msgs::msg::Pose2D & goal,  
      63   const nav_2d_msgs::msg::Path2D & global_plan )
          {
           double dx = pose.x - goal.x;
           double dy = pose.y - goal.y;
           double sq_dist = dx * dx + dy * dy;
           if (  sq_dist > forward_point_distance_ * forward_point_distance_ ) {
           zero_scale_ = false;
           } else {
           // once we are close to goal,   trying to keep the nose close to anything destabilizes behavior.
           zero_scale_ = true;
           return true;
           }
          
           return PathDistCritic::prepare(  pose,   vel,   goal,   global_plan );
          }
          
      79  double PathAlignCritic::getScale(   ) const
          {
           if (  zero_scale_ ) {
           return 0.0;
           } else {
           return costmap_->getResolution(   ) * 0.5 * scale_;
           }
          }
          
      88  double PathAlignCritic::scorePose(  const geometry_msgs::msg::Pose2D & pose )
          {
           return PathDistCritic::scorePose(  getForwardPose(  pose,   forward_point_distance_ ) );
          }
          
          } // namespace dwb_critics
          
      95  PLUGINLIB_EXPORT_CLASS(  dwb_critics::PathAlignCritic,   dwb_core::TrajectoryCritic )

navigation2/nav2_dwb_controller/dwb_critics/src/path_dist.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/path_dist.hpp"
          #include <vector>
          #include "pluginlib/class_list_macros.hpp"
          #include "nav_2d_utils/path_ops.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
          namespace dwb_critics
          {
      43  bool PathDistCritic::prepare(  
      44   const geometry_msgs::msg::Pose2D &,   const nav_2d_msgs::msg::Twist2D &,  
      45   const geometry_msgs::msg::Pose2D &,  
      46   const nav_2d_msgs::msg::Path2D & global_plan )
          {
           reset(   );
           bool started_path = false;
          
           nav_2d_msgs::msg::Path2D adjusted_global_plan =
           nav_2d_utils::adjustPlanResolution(  global_plan,   costmap_->getResolution(   ) );
          
           if (  adjusted_global_plan.poses.size(   ) != global_plan.poses.size(   ) ) {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  
           "PathDistCritic" ),   "Adjusted global plan resolution,   added %zu points",  
           adjusted_global_plan.poses.size(   ) - global_plan.poses.size(   ) );
           }
          
           unsigned int i;
           // put global path points into local map until we reach the border of the local map
           for (  i = 0; i < adjusted_global_plan.poses.size(   ); ++i ) {
           double g_x = adjusted_global_plan.poses[i].x;
           double g_y = adjusted_global_plan.poses[i].y;
           unsigned int map_x,   map_y;
           if (  costmap_->worldToMap(  
           g_x,   g_y,   map_x,  
           map_y ) && costmap_->getCost(  map_x,   map_y ) != nav2_costmap_2d::NO_INFORMATION )
           {
           int index = costmap_->getIndex(  map_x,   map_y );
           cell_values_[index] = 0.0;
           queue_->enqueueCell(  map_x,   map_y );
           started_path = true;
           } else if (  started_path ) {
           break;
           }
           }
           if (  !started_path ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "PathDistCritic" ),  
           "None of the %d first of %zu (  %zu ) points of the global plan were in "
           "the local costmap and free",  
           i,   adjusted_global_plan.poses.size(   ),   global_plan.poses.size(   ) );
           return false;
           }
          
           propogateManhattanDistances(   );
          
           return true;
          }
          
          } // namespace dwb_critics
          
      95  PLUGINLIB_EXPORT_CLASS(  dwb_critics::PathDistCritic,   dwb_core::TrajectoryCritic )

navigation2/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/prefer_forward.hpp"
          #include <math.h>
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_util/node_utils.hpp"
          
      40  PLUGINLIB_EXPORT_CLASS(  dwb_critics::PreferForwardCritic,   dwb_core::TrajectoryCritic )
          
          using nav2_util::declare_parameter_if_not_declared;
          
          namespace dwb_critics
          {
          
      47  void PreferForwardCritic::onInit(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           declare_parameter_if_not_declared(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".penalty",   rclcpp::ParameterValue(  1.0 ) );
           declare_parameter_if_not_declared(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".strafe_x",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + "." + name_ + ".strafe_theta",  
           rclcpp::ParameterValue(  0.2 ) );
           declare_parameter_if_not_declared(  
           node,   dwb_plugin_name_ + "." + name_ + ".theta_scale",  
           rclcpp::ParameterValue(  10.0 ) );
          
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".penalty",   penalty_ );
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".strafe_x",   strafe_x_ );
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".strafe_theta",   strafe_theta_ );
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".theta_scale",   theta_scale_ );
          }
          
      73  double PreferForwardCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           // backward motions bad on a robot without backward sensors
           if (  traj.velocity.x < 0.0 ) {
           return penalty_;
           }
           // strafing motions also bad on such a robot
           if (  traj.velocity.x < strafe_x_ && fabs(  traj.velocity.theta ) < strafe_theta_ ) {
           return penalty_;
           }
          
           // the more we rotate,   the less we progress forward
           return fabs(  traj.velocity.theta ) * theta_scale_;
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/rotate_to_goal.hpp"
          #include <string>
          #include <vector>
          #include "nav_2d_utils/parameters.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "dwb_core/trajectory_utils.hpp"
          #include "angles/angles.h"
          
      44  PLUGINLIB_EXPORT_CLASS(  dwb_critics::RotateToGoalCritic,   dwb_core::TrajectoryCritic )
          
          namespace dwb_critics
          {
          
          inline double hypot_sq(  double dx,   double dy )
          {
           return dx * dx + dy * dy;
          }
          
          void RotateToGoalCritic::onInit(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
          
           xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + ".xy_goal_tolerance",   0.25 );
           xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
           double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + ".trans_stopped_velocity",   0.25 );
           stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
           slowing_factor_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".slowing_factor",   5.0 );
           lookahead_time_ = nav_2d_utils::searchAndGetParam(  
           node,  
           dwb_plugin_name_ + "." + name_ + ".lookahead_time",   -1.0 );
           reset(   );
          }
          
          void RotateToGoalCritic::reset(   )
          {
           in_window_ = false;
           rotating_ = false;
          }
          
          bool RotateToGoalCritic::prepare(  
           const geometry_msgs::msg::Pose2D & pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const geometry_msgs::msg::Pose2D & goal,  
           const nav_2d_msgs::msg::Path2D & )
          {
           double dxy_sq = hypot_sq(  pose.x - goal.x,   pose.y - goal.y );
           in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
           current_xy_speed_sq_ = hypot_sq(  vel.x,   vel.y );
           rotating_ = rotating_ || (  in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_ );
           goal_yaw_ = goal.theta;
           return true;
          }
          
          double RotateToGoalCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           // If we're not sufficiently close to the goal,   we don't care what the twist is
           if (  !in_window_ ) {
           return 0.0;
           } else if (  !rotating_ ) {
           double speed_sq = hypot_sq(  traj.velocity.x,   traj.velocity.y );
           if (  speed_sq >= current_xy_speed_sq_ ) {
           throw dwb_core::IllegalTrajectoryException(  name_,   "Not slowing down near goal." );
           }
           return speed_sq * slowing_factor_ + scoreRotation(  traj );
           }
          
           // If we're sufficiently close to the goal,   any transforming velocity is invalid
           if (  fabs(  traj.velocity.x ) > 0 || fabs(  traj.velocity.y ) > 0 ) {
           throw dwb_core::
           IllegalTrajectoryException(  name_,   "Nonrotation command near goal." );
           }
          
           return scoreRotation(  traj );
          }
          
          double RotateToGoalCritic::scoreRotation(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           if (  traj.poses.empty(   ) ) {
           throw dwb_core::IllegalTrajectoryException(  name_,   "Empty trajectory." );
           }
          
           double end_yaw;
           if (  lookahead_time_ >= 0.0 ) {
           geometry_msgs::msg::Pose2D eval_pose = dwb_core::projectPose(  traj,   lookahead_time_ );
           end_yaw = eval_pose.theta;
           } else {
           end_yaw = traj.poses.back(   ).theta;
           }
           return fabs(  angles::shortest_angular_distance(  end_yaw,   goal_yaw_ ) );
          }
          
          } // namespace dwb_critics

navigation2/nav2_dwb_controller/dwb_critics/src/twirling.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_critics/twirling.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          namespace dwb_critics
          {
      40  void TwirlingCritic::onInit(   )
          {
           auto node = node_.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node"};
           }
           // Scale is set to 0 by default,   so if it was not set otherwise,   set to 0
           node->get_parameter(  dwb_plugin_name_ + "." + name_ + ".scale",   scale_ );
          }
          
      50  double TwirlingCritic::scoreTrajectory(  const dwb_msgs::msg::Trajectory2D & traj )
          {
           return fabs(  traj.velocity.theta ); // add cost for making the robot spin
          }
          } // namespace dwb_critics
          
      56  PLUGINLIB_EXPORT_CLASS(  dwb_critics::TwirlingCritic,   dwb_core::TrajectoryCritic )

navigation2/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020,   Samsung Research America
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <vector>
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_critics/alignment_util.hpp"
          #include "dwb_core/exceptions.hpp"
          
      44  TEST(  AlignmentUtil,   TestProjection )
          {
           geometry_msgs::msg::Pose2D pose,   pose_out;
           pose.x = 1.0;
           pose.y = -1.0;
           double distance = 1.0;
           pose_out = dwb_critics::getForwardPose(  pose,   distance );
           EXPECT_EQ(  pose_out.x,   2.0 );
           EXPECT_EQ(  pose_out.y,   -1.0 );
           EXPECT_EQ(  pose_out.theta,   pose.theta );
          
           pose.x = 2.0;
           pose.y = -10.0;
           pose.theta = 0.54;
           pose_out = dwb_critics::getForwardPose(  pose,   distance );
           EXPECT_NEAR(  pose_out.x,   2.8577,   0.01 );
           EXPECT_NEAR(  pose_out.y,   -9.4858,   0.01 );
           EXPECT_EQ(  pose_out.theta,   pose.theta );
          }
          
      64  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <vector>
          #include <memory>
          #include <string>
          #include <utility>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_critics/obstacle_footprint.hpp"
          #include "dwb_core/exceptions.hpp"
          
      45  TEST(  BaseObstacle,   IsValidCost )
          {
           std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
           std::make_shared<dwb_critics::BaseObstacleCritic>(   );
          
           for (  int i = 0; i < 256; i++ ) {
           // for these 3 values the cost is not "valid"
           if (  i == nav2_costmap_2d::LETHAL_OBSTACLE ||
           i == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
           i == nav2_costmap_2d::NO_INFORMATION )
           {
           ASSERT_FALSE(  critic->isValidCost(  i ) );
           } else {
           ASSERT_TRUE(  critic->isValidCost(  i ) );
           }
           }
          }
          
      63  TEST(  BaseObstacle,   ScorePose )
          {
           std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
           std::make_shared<dwb_critics::BaseObstacleCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "base_obstacle_critic_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
          
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           costmap_ros->getCostmap(   )->setCost(  0,   0,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  0,   1,   nav2_costmap_2d::NO_INFORMATION );
           const int some_other_cost = 128;
           costmap_ros->getCostmap(   )->setCost(  0,   2,   some_other_cost );
          
           // The pose is in "world" coordinates. The (  default ) resolution is 0.1 m.
           geometry_msgs::msg::Pose2D pose;
           pose.x = 0;
           pose.y = 0;
          
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = 0;
           pose.y = 0.15;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.y = 0.25;
           pose.x = 0.05;
           ASSERT_EQ(  critic->scorePose(  pose ),   some_other_cost );
          
           // The theta should not influence the cost
           for (  int i = -50; i < 150; i++ ) {
           pose.theta = (  1.0 / 50 ) * i * M_PI;
           ASSERT_EQ(  critic->scorePose(  pose ),   some_other_cost );
           }
          
           // Poses outside the map should throw an exception.
           pose.x = 1.0;
           pose.y = -0.1;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = costmap_ros->getCostmap(   )->getSizeInMetersX(   ) + 0.1;
           pose.y = 1.0;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = 1.0;
           pose.y = costmap_ros->getCostmap(   )->getSizeInMetersY(   ) + 0.1;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = -0.1;
           pose.y = 1.0;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          }
          
     122  TEST(  BaseObstacle,   CriticVisualization )
          {
           std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
           std::make_shared<dwb_critics::BaseObstacleCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "base_obstacle_critic_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
          
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           costmap_ros->getCostmap(   )->setCost(  0,   0,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  0,   1,   nav2_costmap_2d::NO_INFORMATION );
           // Some random values
           costmap_ros->getCostmap(   )->setCost(  3,   2,   64 );
           costmap_ros->getCostmap(   )->setCost(  30,   12,   85 );
           costmap_ros->getCostmap(   )->setCost(  10,   49,   24 );
           costmap_ros->getCostmap(   )->setCost(  45,   2,   12 );
          
           std::vector<std::pair<std::string,   std::vector<float>>> cost_channels;
           critic->addCriticVisualization(  cost_channels );
          
           unsigned int size_x = costmap_ros->getCostmap(   )->getSizeInCellsX(   );
           unsigned int size_y = costmap_ros->getCostmap(   )->getSizeInCellsY(   );
          
           // The values in the pointcloud should be equal to the values in the costmap
           for (  unsigned int y = 0; y < size_y; y++ ) {
           for (  unsigned int x = 0; x < size_x; x++ ) {
           float pointValue = cost_channels[0].second[y * size_y + x];
           ASSERT_EQ(  static_cast<int>(  pointValue ),   costmap_ros->getCostmap(   )->getCost(  x,   y ) );
           }
           }
          }
          
     160  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <vector>
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_critics/obstacle_footprint.hpp"
          #include "dwb_core/exceptions.hpp"
          
      44  class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic
          {
          public:
      47   double pointCost(  int x,   int y )
           {
           return dwb_critics::ObstacleFootprintCritic::pointCost(  x,   y );
           }
          
      52   double lineCost(  int x0,   int x1,   int y0,   int y1 )
           {
           return dwb_critics::ObstacleFootprintCritic::lineCost(  x0,   x1,   y0,   y1 );
           }
          };
          
          // Rotate the given point for angle radians around the origin.
      59  geometry_msgs::msg::Point rotate_origin(  geometry_msgs::msg::Point p,   double angle )
          {
           double s = sin(  angle );
           double c = cos(  angle );
          
           // rotate point
           double xnew = p.x * c - p.y * s;
           double ynew = p.x * s + p.y * c;
          
           p.x = xnew;
           p.y = ynew;
          
           return p;
          }
          
          // Auxilary function to create a Point with given x and y values.
      75  geometry_msgs::msg::Point getPoint(  double x,   double y )
          {
           geometry_msgs::msg::Point p;
           p.x = x;
           p.y = y;
           return p;
          }
          
          // Variables
          double footprint_size_x_half = 1.8;
          double footprint_size_y_half = 1.6;
          
      87  std::vector<geometry_msgs::msg::Point> getFootprint(   )
          {
           std::vector<geometry_msgs::msg::Point> footprint;
           footprint.push_back(  getPoint(  footprint_size_x_half,   footprint_size_y_half ) );
           footprint.push_back(  getPoint(  footprint_size_x_half,   -footprint_size_y_half ) );
           footprint.push_back(  getPoint(  -footprint_size_x_half,   -footprint_size_y_half ) );
           footprint.push_back(  getPoint(  -footprint_size_x_half,   footprint_size_y_half ) );
           return footprint;
          }
          
      97  TEST(  ObstacleFootprint,   GetOrientedFootprint )
          {
           double theta = 0.1234;
          
           std::vector<geometry_msgs::msg::Point> footprint_before = getFootprint(   );
          
           std::vector<geometry_msgs::msg::Point> footprint_after;
           geometry_msgs::msg::Pose2D pose;
           pose.theta = theta;
           footprint_after = dwb_critics::getOrientedFootprint(  pose,   footprint_before );
          
           uint i;
           for (  i = 0; i < footprint_before.size(   ); i++ ) {
           ASSERT_EQ(  rotate_origin(  footprint_before[i],   theta ),   footprint_after[i] );
           }
          
           theta = 5.123;
           pose.theta = theta;
           footprint_after = dwb_critics::getOrientedFootprint(  pose,   footprint_before );
          
           for (  unsigned int i = 0; i < footprint_before.size(   ); i++ ) {
           ASSERT_EQ(  rotate_origin(  footprint_before[i],   theta ),   footprint_after[i] );
           }
          }
          
     122  TEST(  ObstacleFootprint,   Prepare )
          {
           std::shared_ptr<dwb_critics::ObstacleFootprintCritic> critic =
           std::make_shared<dwb_critics::ObstacleFootprintCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           geometry_msgs::msg::Pose2D pose;
           nav_2d_msgs::msg::Twist2D vel;
           geometry_msgs::msg::Pose2D goal;
           nav_2d_msgs::msg::Path2D global_plan;
          
           // no footprint set in the costmap. Prepare should return false;
           std::vector<geometry_msgs::msg::Point> footprint;
           costmap_ros->setRobotFootprint(  footprint );
           ASSERT_FALSE(  critic->prepare(  pose,   vel,   goal,   global_plan ) );
          
           costmap_ros->setRobotFootprint(  getFootprint(   ) );
           ASSERT_TRUE(  critic->prepare(  pose,   vel,   goal,   global_plan ) );
          
           double epsilon = 0.01;
           // If the robot footprint goes of the map,   it should throw an exception
           // The following cases put the robot over the edge of the map on the left,   bottom,   right and top
          
           pose.x = footprint_size_x_half; // This gives an error
           pose.y = footprint_size_y_half + epsilon;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = footprint_size_x_half + epsilon;
           pose.y = footprint_size_y_half; // error
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = costmap_ros->getCostmap(   )->getSizeInMetersX(   ) - footprint_size_x_half; // error
           pose.y = costmap_ros->getCostmap(   )->getSizeInMetersY(   ) + footprint_size_y_half - epsilon;
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = costmap_ros->getCostmap(   )->getSizeInMetersX(   ) - footprint_size_x_half - epsilon;
           pose.y = costmap_ros->getCostmap(   )->getSizeInMetersY(   ) + footprint_size_y_half; // error
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          
           pose.x = footprint_size_x_half + epsilon;
           pose.y = footprint_size_y_half + epsilon;
           ASSERT_EQ(  critic->scorePose(  pose ),   0.0 );
          
           for (  unsigned int i = 1; i < costmap_ros->getCostmap(   )->getSizeInCellsX(   ); i++ ) {
           costmap_ros->getCostmap(   )->setCost(  i,   10,   nav2_costmap_2d::LETHAL_OBSTACLE );
           }
           // It should now hit an obstacle (  throw an expection )
           ASSERT_THROW(  critic->scorePose(  pose ),   dwb_core::IllegalTrajectoryException );
          }
          
          // todo: wilcobonestroo Add tests for other footprint shapes and costmaps.
          
     182  TEST(  ObstacleFootprint,   PointCost )
          {
           std::shared_ptr<OpenObstacleFootprintCritic> critic =
           std::make_shared<OpenObstacleFootprintCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           costmap_ros->getCostmap(   )->setCost(  0,   0,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  0,   1,   nav2_costmap_2d::NO_INFORMATION );
           costmap_ros->getCostmap(   )->setCost(  0,   2,   128 );
          
           ASSERT_THROW(  critic->pointCost(  0,   0 ),   dwb_core::IllegalTrajectoryException );
           ASSERT_THROW(  critic->pointCost(  0,   1 ),   dwb_core::IllegalTrajectoryException );
           ASSERT_EQ(  critic->pointCost(  0,   2 ),   128 );
          }
          
     205  TEST(  ObstacleFootprint,   LineCost )
          {
           std::shared_ptr<OpenObstacleFootprintCritic> critic =
           std::make_shared<OpenObstacleFootprintCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           costmap_ros->getCostmap(   )->setCost(  3,   3,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  3,   4,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  4,   3,   nav2_costmap_2d::LETHAL_OBSTACLE );
           costmap_ros->getCostmap(   )->setCost(  4,   4,   nav2_costmap_2d::LETHAL_OBSTACLE );
          
           ASSERT_THROW(  critic->lineCost(  0,   5,   2,   6 ),   dwb_core::IllegalTrajectoryException );
           ASSERT_THROW(  critic->lineCost(  5,   0,   6,   2 ),   dwb_core::IllegalTrajectoryException );
          
           ASSERT_THROW(  critic->lineCost(  2,   4,   0,   10 ),   dwb_core::IllegalTrajectoryException );
           ASSERT_THROW(  critic->lineCost(  4,   2,   10,   0 ),   dwb_core::IllegalTrajectoryException );
          
           // These all miss the obstacle
           ASSERT_EQ(  critic->lineCost(  2,   2,   0,   10 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  2,   2,   10,   0 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  5,   5,   0,   10 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  5,   5,   10,   0 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  0,   50,   2,   2 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  50,   0,   2,   2 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  0,   50,   5,   5 ),   0.0 );
           ASSERT_EQ(  critic->lineCost(  50,   0,   5,   5 ),   0.0 );
          
           // Use valid costs
           costmap_ros->getCostmap(   )->setCost(  3,   3,   50 );
           costmap_ros->getCostmap(   )->setCost(  3,   4,   50 );
           costmap_ros->getCostmap(   )->setCost(  4,   3,   100 );
           costmap_ros->getCostmap(   )->setCost(  4,   4,   100 );
          
           ASSERT_EQ(  critic->lineCost(  3,   3,   0,   50 ),   50 ); // all 50
           ASSERT_EQ(  critic->lineCost(  4,   4,   0,   10 ),   100 ); // all 100
           ASSERT_EQ(  critic->lineCost(  0,   50,   3,   3 ),   100 ); // pass 50 and 100
          }
          
     251  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <math.h>
          #include <vector>
          
          #include <memory>
          #include <string>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_critics/prefer_forward.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          
      49  static constexpr double default_penalty = 1.0;
      50  static constexpr double default_strafe_x = 0.1;
      51  static constexpr double default_strafe_theta = 0.2;
      52  static constexpr double default_theta_scale = 10.0;
          
          TEST(  PreferForward,   StartNode )
          {
           auto critic = std::make_shared<dwb_critics::PreferForwardCritic>(   );
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
           node->configure(   );
           node->activate(   );
          
           std::string name = "test";
           std::string ns = "ns";
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           EXPECT_EQ(  node->get_parameter(  ns + "." + name + ".penalty" ).as_double(   ),   default_penalty );
           EXPECT_EQ(  node->get_parameter(  ns + "." + name + ".strafe_x" ).as_double(   ),   default_strafe_x );
           EXPECT_EQ(  
           node->get_parameter(  ns + "." + name + ".strafe_theta" ).as_double(   ),   default_strafe_theta );
           EXPECT_EQ(  node->get_parameter(  ns + "." + name + ".theta_scale" ).as_double(   ),   default_theta_scale );
          }
          
          TEST(  PreferForward,   NegativeVelocityX )
          {
           auto critic = std::make_shared<dwb_critics::PreferForwardCritic>(   );
           dwb_msgs::msg::Trajectory2D trajectory;
          
           // score must be equal to the penalty (  1.0 ) for any negative x velocity
           trajectory.velocity.x = -1.0;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = -0.00001;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = -0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = std::numeric_limits<double>::lowest(   );
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = -std::numeric_limits<double>::min(   );
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          }
          
          TEST(  PreferForward,   Strafe )
          {
           auto critic = std::make_shared<dwb_critics::PreferForwardCritic>(   );
           dwb_msgs::msg::Trajectory2D trajectory;
          
           // score must be equal to the penalty (  1.0 ) when x vel is lower than 0.1
           // and theta is between -0.2 and 0.2
           trajectory.velocity.x = 0.05;
           trajectory.velocity.theta = -0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = 0.0999999;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.x = 0.000001;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.theta = -0.19;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          
           trajectory.velocity.theta = 0.19;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   default_penalty );
          }
          
          TEST(  PreferForward,   Normal )
          {
           auto critic = std::make_shared<dwb_critics::PreferForwardCritic>(   );
           dwb_msgs::msg::Trajectory2D trajectory;
          
           // score must be equal to the theta * scaling factor (  10.0 )
           trajectory.velocity.x = 0.2;
           trajectory.velocity.theta = -0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.1 * default_theta_scale );
          
           trajectory.velocity.theta = 0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.1 * default_theta_scale );
          
           trajectory.velocity.theta = -0.2;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.2 * default_theta_scale );
          
           trajectory.velocity.theta = 0.2;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.2 * default_theta_scale );
          
           trajectory.velocity.theta = 1.5;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   1.5 * default_theta_scale );
          }
          
          TEST(  PreferForward,   NoneDefaultValues )
          {
           auto critic = std::make_shared<dwb_critics::PreferForwardCritic>(   );
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
           node->configure(   );
           node->activate(   );
          
           double penalty = 18.3;
           double strafe_x = 0.5;
           double strafe_theta = 0.4;
           double theta_scale = 15.0;
          
           std::string name = "test";
           std::string ns = "ns";
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   ns + "." + name + ".penalty",  
           rclcpp::ParameterValue(  penalty ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   ns + "." + name + ".strafe_x",  
           rclcpp::ParameterValue(  strafe_x ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   ns + "." + name + ".strafe_theta",  
           rclcpp::ParameterValue(  strafe_theta ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   ns + "." + name + ".theta_scale",  
           rclcpp::ParameterValue(  theta_scale ) );
          
           critic->initialize(  node,   name,   ns,   costmap_ros );
           critic->onInit(   );
          
           dwb_msgs::msg::Trajectory2D trajectory;
           trajectory.velocity.x = 0.05;
           trajectory.velocity.theta = -0.1;
          
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           // score must be equal to the penalty when x vel is lower than strafe_x
           // and theta is between -strafe_theta and strafe_theta
           trajectory.velocity.x = 0.4;
           trajectory.velocity.theta = -0.39;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           trajectory.velocity.x = 0.0999999;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           trajectory.velocity.x = 0.000001;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           trajectory.velocity.theta = -0.09999;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           trajectory.velocity.theta = 0.09999;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   penalty );
          
           // score must be equal to the theta * scaling factor (  10.0 )
           trajectory.velocity.x = 0.5;
           trajectory.velocity.theta = -0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.1 * theta_scale );
          
           trajectory.velocity.theta = 0.1;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.1 * theta_scale );
          
           trajectory.velocity.theta = -0.2;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.2 * theta_scale );
          
           trajectory.velocity.theta = 0.2;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   0.2 * theta_scale );
          
           trajectory.velocity.theta = 1.5;
           EXPECT_EQ(  critic->scoreTrajectory(  trajectory ),   1.5 * theta_scale );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020,   Samsung Research America
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <vector>
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_critics/twirling.hpp"
          #include "dwb_core/exceptions.hpp"
          
      44  TEST(  TwirlingTests,   Scoring )
          {
           std::shared_ptr<dwb_critics::TwirlingCritic> critic =
           std::make_shared<dwb_critics::TwirlingCritic>(   );
          
           auto node = nav2_util::LifecycleNode::make_shared(  "costmap_tester" );
          
           auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "test_global_costmap" );
           costmap_ros->configure(   );
          
           std::string name = "name";
           std::string ns = "ns";
           critic->initialize(  node,   name,   ns,   costmap_ros );
          
           dwb_msgs::msg::Trajectory2D traj;
           traj.velocity.theta = 1.0;
           EXPECT_EQ(  critic->scoreTrajectory(  traj ),   1.0 );
           traj.velocity.theta = -1.0;
           EXPECT_EQ(  critic->scoreTrajectory(  traj ),   1.0 );
          }
          
      65  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
          #define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_plugins
          {
          
          /**
           * @struct KinematicParameters
           * @brief A struct containing one representation of the robot's kinematics
           */
          struct KinematicParameters
          {
      54   friend class KinematicsHandler;
          
           inline double getMinX(   ) {return min_vel_x_;}
           inline double getMaxX(   ) {return max_vel_x_;}
           inline double getAccX(   ) {return acc_lim_x_;}
           inline double getDecelX(   ) {return decel_lim_x_;}
          
           inline double getMinY(   ) {return min_vel_y_;}
           inline double getMaxY(   ) {return max_vel_y_;}
           inline double getAccY(   ) {return acc_lim_y_;}
           inline double getDecelY(   ) {return decel_lim_y_;}
          
           inline double getMinSpeedXY(   ) {return min_speed_xy_;}
           inline double getMaxSpeedXY(   ) {return max_speed_xy_;}
          
           inline double getMinTheta(   ) {return -max_vel_theta_;}
           inline double getMaxTheta(   ) {return max_vel_theta_;}
           inline double getAccTheta(   ) {return acc_lim_theta_;}
           inline double getDecelTheta(   ) {return decel_lim_theta_;}
           inline double getMinSpeedTheta(   ) {return min_speed_theta_;}
          
           inline double getMinSpeedXY_SQ(   ) {return min_speed_xy_sq_;}
           inline double getMaxSpeedXY_SQ(   ) {return max_speed_xy_sq_;}
          
          protected:
           // For parameter descriptions,   see cfg/KinematicParams.cfg
           double min_vel_x_{0};
           double min_vel_y_{0};
           double max_vel_x_{0};
           double max_vel_y_{0};
           double base_max_vel_x_{0};
           double base_max_vel_y_{0};
           double max_vel_theta_{0};
           double base_max_vel_theta_{0};
           double min_speed_xy_{0};
           double max_speed_xy_{0};
           double base_max_speed_xy_{0};
           double min_speed_theta_{0};
           double acc_lim_x_{0};
           double acc_lim_y_{0};
           double acc_lim_theta_{0};
           double decel_lim_x_{0};
           double decel_lim_y_{0};
           double decel_lim_theta_{0};
          
           // Cached square values of min_speed_xy and max_speed_xy
           double min_speed_xy_sq_{0};
           double max_speed_xy_sq_{0};
          };
          
          /**
           * @class KinematicsHandler
           * @brief A class managing the representation of the robot's kinematics
           */
          class KinematicsHandler
          {
          public:
           KinematicsHandler(   );
           ~KinematicsHandler(   );
           void initialize(  const nav2_util::LifecycleNode::SharedPtr & nh,   const std::string & plugin_name );
          
           inline KinematicParameters getKinematics(   ) {return *kinematics_.load(   );}
          
           void setSpeedLimit(  const double & speed_limit,   const bool & percentage );
          
           using Ptr = std::shared_ptr<KinematicsHandler>;
          
          protected:
           std::atomic<KinematicParameters *> kinematics_;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
           void update_kinematics(  KinematicParameters kinematics );
           std::string plugin_name_;
          };
          
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
          #define DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
          
          #include <memory>
          #include <string>
          
          #include "dwb_plugins/standard_traj_generator.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_plugins
          {
          /**
           * @class LimitedAccelGenerator
           * @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
           */
      50  class LimitedAccelGenerator : public StandardTrajectoryGenerator
          {
          public:
           void initialize(  
           const nav2_util::LifecycleNode::SharedPtr & nh,  
           const std::string & plugin_name ) override;
           void startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity ) override;
          
          protected:
           /**
           * @brief Calculate the velocity after a set period of time,   given the desired velocity and acceleration limits
           *
           * Unlike the StandardTrajectoryGenerator,   the velocity remains constant in the LimitedAccelGenerator
           *
           * @param cmd_vel Desired velocity
           * @param start_vel starting velocity
           * @param dt amount of time in seconds
           * @return cmd_vel
           */
           nav_2d_msgs::msg::Twist2D computeNewVelocity(  
           const nav_2d_msgs::msg::Twist2D & cmd_vel,  
           const nav_2d_msgs::msg::Twist2D & start_vel,  
           const double dt ) override;
           double acceleration_time_;
           std::string plugin_name_;
          };
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
          #define DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
          
          #include <algorithm>
          #include <cmath>
          
          namespace dwb_plugins
          {
          
          const double EPSILON = 1E-5;
          
          /**
           * @brief Given initial conditions and a time,   figure out the end velocity
           *
           * @param v0 Initial velocity
           * @param accel The acceleration rate
           * @param decel The decceleration rate
           * @param dt Delta time - amount of time to project into the future
           * @param target target velocity
           * @return The velocity dt seconds after v0.
           */
      56  inline double projectVelocity(  double v0,   double accel,   double decel,   double dt,   double target )
          {
           double v1;
           if (  v0 < target ) {
           v1 = v0 + accel * dt;
           return std::min(  target,   v1 );
           } else {
           v1 = v0 + decel * dt;
           return std::max(  target,   v1 );
           }
          }
          
          /**
           * @class OneDVelocityIterator
           * @brief An iterator for generating a number of samples in a range
           *
           * In its simplest usage,   this gives us N (  num_samples ) different velocities that are reachable
           * given our current velocity. However,   there is some fancy logic around zero velocities and
           * the min/max velocities
           *
           * If the current velocity is 2 m/s,   and the acceleration limit is 1 m/ss and the acc_time is 1 s,  
           * this class would provide velocities between 1 m/s and 3 m/s.
           *
           *
           *
           */
      82  class OneDVelocityIterator
          {
          public:
           /**
           * @brief Constructor for the velocity iterator
           *
           * @param current Current velocity
           * @param min Minimum velocity allowable
           * @param max Maximum velocity allowable
           * @param acc_limit Acceleration Limit
           * @param decel_limit Deceleration Limit
           * @param num_samples The number of samples to return
           */
      95   OneDVelocityIterator(  
           double current,   double min,   double max,   double acc_limit,   double decel_limit,   double acc_time,  
           int num_samples )
           {
           if (  current < min ) {
           current = min;
           } else if (  current > max ) {
           current = max;
           }
           max_vel_ = projectVelocity(  current,   acc_limit,   decel_limit,   acc_time,   max );
           min_vel_ = projectVelocity(  current,   acc_limit,   decel_limit,   acc_time,   min );
           reset(   );
          
           if (  fabs(  min_vel_ - max_vel_ ) < EPSILON ) {
           increment_ = 1.0;
           return;
           }
           num_samples = std::max(  2,   num_samples );
          
           // e.g. for 4 samples,   split distance in 3 even parts
           increment_ = (  max_vel_ - min_vel_ ) / std::max(  1,   (  num_samples - 1 ) );
           }
          
           /**
           * @brief Get the next velocity available
           */
     121   double getVelocity(   ) const
           {
           if (  return_zero_now_ ) {return 0.0;}
           return current_;
           }
          
           /**
           * @brief Increment the iterator
           */
     130   OneDVelocityIterator & operator++(   )
           {
           if (  return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 &&
           current_ + increment_ <= max_vel_ + EPSILON )
           {
           return_zero_now_ = true;
           return_zero_ = false;
           } else {
           current_ += increment_;
           return_zero_now_ = false;
           }
           return *this;
           }
          
           /**
           * @brief Reset back to the first velocity
           */
     147   void reset(   )
           {
           current_ = min_vel_;
           return_zero_ = true;
           return_zero_now_ = false;
           }
          
           /**
           * If we have returned all the velocities for this iteration
           */
     157   bool isFinished(   ) const
           {
           return current_ > max_vel_ + EPSILON;
           }
          
          private:
     163   bool return_zero_,   return_zero_now_;
           double min_vel_,   max_vel_;
           double current_;
           double increment_;
          };
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp

          /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
          #define DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
          
          #include <vector>
          #include <memory>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "dwb_core/trajectory_generator.hpp"
          #include "dwb_plugins/velocity_iterator.hpp"
          #include "dwb_plugins/kinematic_parameters.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_plugins
          {
          
          /**
           * @class StandardTrajectoryGenerator
           * @brief Standard DWA-like trajectory generator.
           */
      55  class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
          {
          public:
           // Standard TrajectoryGenerator interface
           void initialize(  
           const nav2_util::LifecycleNode::SharedPtr & nh,  
           const std::string & plugin_name ) override;
           void startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity ) override;
           bool hasMoreTwists(   ) override;
           nav_2d_msgs::msg::Twist2D nextTwist(   ) override;
          
           dwb_msgs::msg::Trajectory2D generateTrajectory(  
           const geometry_msgs::msg::Pose2D & start_pose,  
           const nav_2d_msgs::msg::Twist2D & start_vel,  
           const nav_2d_msgs::msg::Twist2D & cmd_vel ) override;
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
           void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) override
           {
           if (  kinematics_handler_ ) {
           kinematics_handler_->setSpeedLimit(  speed_limit,   percentage );
           }
           }
          
          protected:
           /**
           * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
           */
           virtual void initializeIterator(  const nav2_util::LifecycleNode::SharedPtr & nh );
          
           /**
           * @brief Calculate the velocity after a set period of time,   given the desired velocity and acceleration limits
           *
           * @param cmd_vel Desired velocity
           * @param start_vel starting velocity
           * @param dt amount of time in seconds
           * @return new velocity after dt seconds
           */
      99   virtual nav_2d_msgs::msg::Twist2D computeNewVelocity(  
           const nav_2d_msgs::msg::Twist2D & cmd_vel,   const nav_2d_msgs::msg::Twist2D & start_vel,  
           const double dt );
          
           /**
           * @brief Use the robot's kinematic model to predict new positions for the robot
           *
           * @param start_pose Starting pose
           * @param vel Actual robot velocity (  assumed to be within acceleration limits )
           * @param dt amount of time in seconds
           * @return New pose after dt seconds
           */
           virtual geometry_msgs::msg::Pose2D computeNewPosition(  
           const geometry_msgs::msg::Pose2D start_pose,   const nav_2d_msgs::msg::Twist2D & vel,  
           const double dt );
          
          
           /**
           * @brief Compute an array of time deltas between the points in the generated trajectory.
           *
           * @param cmd_vel The desired command velocity
           * @return vector of the difference between each time step in the generated trajectory
           *
           * If we are discretizing by time,   the returned vector will be the same constant time_granularity
           * for all cmd_vels. Otherwise,   you will get times based on the linear/angular granularity.
           *
           * Right now the vector contains a single value repeated many times,   but this method could be overridden
           * to allow for dynamic spacing
           */
           virtual std::vector<double> getTimeSteps(  const nav_2d_msgs::msg::Twist2D & cmd_vel );
          
           KinematicsHandler::Ptr kinematics_handler_;
           std::shared_ptr<VelocityIterator> velocity_iterator_;
          
           double sim_time_;
          
           // Sampling Parameters
           bool discretize_by_time_;
          
           /// @brief If discretizing by time,   the amount of time between each point in the traj
           double time_granularity_;
          
           /// @brief If not discretizing by time,   the amount of linear space between points
           double linear_granularity_;
          
           /// @brief If not discretizing by time,   the amount of angular space between points
           double angular_granularity_;
          
           /// @brief the name of the overlying plugin ID
           std::string plugin_name_;
          
           /* Backwards Compatibility Parameter: include_last_point
           *
           * dwa had an off-by-one error built into it.
           * It generated N trajectory points,   where N = ceil(  sim_time / time_delta ).
           * If for example,   sim_time=3.0 and time_delta=1.5,   it would generate trajectories with 2 points,   which
           * indeed were time_delta seconds apart. However,   the points would be at t=0 and t=1.5,   and thus the
           * actual sim_time was much less than advertised.
           *
           * This is remedied by adding one final point at t=sim_time,   but only if include_last_point_ is true.
           *
           * Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
           * were not projected out as far as they intended.
           */
           bool include_last_point_;
          };
          
          
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
          #define DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
          
          #include <memory>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav_2d_msgs/msg/twist2_d.hpp"
          #include "dwb_plugins/kinematic_parameters.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_plugins
          {
      48  class VelocityIterator
          {
          public:
      51   virtual ~VelocityIterator(   ) {}
      52   virtual void initialize(  
      53   const nav2_util::LifecycleNode::SharedPtr & nh,  
      54   KinematicsHandler::Ptr kinematics,  
      55   const std::string & plugin_name ) = 0;
      56   virtual void startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity,   double dt ) = 0;
      57   virtual bool hasMoreTwists(   ) = 0;
      58   virtual nav_2d_msgs::msg::Twist2D nextTwist(   ) = 0;
          };
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__VELOCITY_ITERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
          #define DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
          
          #include <memory>
          #include <string>
          
          #include "dwb_plugins/velocity_iterator.hpp"
          #include "dwb_plugins/one_d_velocity_iterator.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          
          namespace dwb_plugins
          {
      47  class XYThetaIterator : public VelocityIterator
          {
          public:
      50   XYThetaIterator(   )
           : kinematics_handler_(  nullptr ),   x_it_(  nullptr ),   y_it_(  nullptr ),   th_it_(  nullptr ) {}
           void initialize(  
           const nav2_util::LifecycleNode::SharedPtr & nh,  
           KinematicsHandler::Ptr kinematics,  
           const std::string & plugin_name ) override;
           void startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity,   double dt ) override;
           bool hasMoreTwists(   ) override;
           nav_2d_msgs::msg::Twist2D nextTwist(   ) override;
          
          protected:
           /**
           * @brief Check to see whether the combined x/y/theta velocities are valid
           * @return True if the magnitude hypot(  x,  y ) and theta are within the robot's absolute limits
           *
           * This is based on three parameters: min_speed_xy,   max_speed_xy and min_speed_theta.
           * The speed is valid if
           * 1 ) The combined magnitude hypot(  x,  y ) is less than max_speed_xy (  or max_speed_xy is negative )
           * AND
           * 2 ) min_speed_xy is negative or min_speed_theta is negative or
           * hypot(  x,  y ) is greater than min_speed_xy or fabs(  theta ) is greater than min_speed_theta.
           */
           bool isValidSpeed(  double x,   double y,   double theta );
           virtual bool isValidVelocity(   );
           void iterateToValidVelocity(   );
           int vx_samples_,   vy_samples_,   vtheta_samples_;
           KinematicsHandler::Ptr kinematics_handler_;
          
           std::shared_ptr<OneDVelocityIterator> x_it_,   y_it_,   th_it_;
          };
          } // namespace dwb_plugins
          
          #endif // DWB_PLUGINS__XY_THETA_ITERATOR_HPP_

navigation2/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_plugins/kinematic_parameters.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav_2d_utils/parameters.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          using nav2_util::declare_parameter_if_not_declared;
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace dwb_plugins
          {
          
      52  KinematicsHandler::KinematicsHandler(   )
          {
           kinematics_.store(  new KinematicParameters );
          }
          
      57  KinematicsHandler::~KinematicsHandler(   )
          {
           delete kinematics_.load(   );
          }
          
      62  void KinematicsHandler::initialize(  
      63   const nav2_util::LifecycleNode::SharedPtr & nh,  
      64   const std::string & plugin_name )
          {
           plugin_name_ = plugin_name;
          
           declare_parameter_if_not_declared(  nh,   plugin_name + ".min_vel_x",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".min_vel_y",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".max_vel_x",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".max_vel_y",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".max_vel_theta",  
           rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".min_speed_xy",  
           rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".max_speed_xy",  
           rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".min_speed_theta",  
           rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".acc_lim_x",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".acc_lim_y",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".acc_lim_theta",  
           rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".decel_lim_x",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  nh,   plugin_name + ".decel_lim_y",   rclcpp::ParameterValue(  0.0 ) );
           declare_parameter_if_not_declared(  
           nh,   plugin_name + ".decel_lim_theta",  
           rclcpp::ParameterValue(  0.0 ) );
          
           KinematicParameters kinematics;
          
           nh->get_parameter(  plugin_name + ".min_vel_x",   kinematics.min_vel_x_ );
           nh->get_parameter(  plugin_name + ".min_vel_y",   kinematics.min_vel_y_ );
           nh->get_parameter(  plugin_name + ".max_vel_x",   kinematics.max_vel_x_ );
           nh->get_parameter(  plugin_name + ".max_vel_y",   kinematics.max_vel_y_ );
           nh->get_parameter(  plugin_name + ".max_vel_theta",   kinematics.max_vel_theta_ );
           nh->get_parameter(  plugin_name + ".min_speed_xy",   kinematics.min_speed_xy_ );
           nh->get_parameter(  plugin_name + ".max_speed_xy",   kinematics.max_speed_xy_ );
           nh->get_parameter(  plugin_name + ".min_speed_theta",   kinematics.min_speed_theta_ );
           nh->get_parameter(  plugin_name + ".acc_lim_x",   kinematics.acc_lim_x_ );
           nh->get_parameter(  plugin_name + ".acc_lim_y",   kinematics.acc_lim_y_ );
           nh->get_parameter(  plugin_name + ".acc_lim_theta",   kinematics.acc_lim_theta_ );
           nh->get_parameter(  plugin_name + ".decel_lim_x",   kinematics.decel_lim_x_ );
           nh->get_parameter(  plugin_name + ".decel_lim_y",   kinematics.decel_lim_y_ );
           nh->get_parameter(  plugin_name + ".decel_lim_theta",   kinematics.decel_lim_theta_ );
          
           kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
           kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
           kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
           kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = nh->add_on_set_parameters_callback(  
           std::bind(  &KinematicsHandler::dynamicParametersCallback,   this,   _1 ) );
          
           kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
           kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
          
           update_kinematics(  kinematics );
          }
          
     127  void KinematicsHandler::setSpeedLimit(  
     128   const double & speed_limit,   const bool & percentage )
          {
           KinematicParameters kinematics(  *kinematics_.load(   ) );
          
           if (  speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT ) {
           // Restore default value
           kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_;
           kinematics.max_vel_x_ = kinematics.base_max_vel_x_;
           kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
           kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
           } else {
           if (  percentage ) {
           // Speed limit is expressed in % from maximum speed of robot
           kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
           kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
           kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
           kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
           } else {
           // Speed limit is expressed in absolute value
           if (  speed_limit < kinematics.base_max_speed_xy_ ) {
           kinematics.max_speed_xy_ = speed_limit;
           // Handling components and angular velocity changes:
           // Max velocities are being changed in the same proportion
           // as absolute linear speed changed in order to preserve
           // robot moving trajectories to be the same after speed change.
           const double ratio = speed_limit / kinematics.base_max_speed_xy_;
           kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
           kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
           kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
           }
           }
           }
          
           // Do not forget to update max_speed_xy_sq_ as well
           kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
          
           update_kinematics(  kinematics );
          }
          
          rcl_interfaces::msg::SetParametersResult
     168  KinematicsHandler::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           KinematicParameters kinematics(  *kinematics_.load(   ) );
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".min_vel_x" ) {
           kinematics.min_vel_x_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".min_vel_y" ) {
           kinematics.min_vel_y_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".max_vel_x" ) {
           kinematics.max_vel_x_ = parameter.as_double(   );
           kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
           } else if (  name == plugin_name_ + ".max_vel_y" ) {
           kinematics.max_vel_y_ = parameter.as_double(   );
           kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
           } else if (  name == plugin_name_ + ".max_vel_theta" ) {
           kinematics.max_vel_theta_ = parameter.as_double(   );
           kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
           } else if (  name == plugin_name_ + ".min_speed_xy" ) {
           kinematics.min_speed_xy_ = parameter.as_double(   );
           kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
           } else if (  name == plugin_name_ + ".max_speed_xy" ) {
           kinematics.max_speed_xy_ = parameter.as_double(   );
           kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
           } else if (  name == plugin_name_ + ".min_speed_theta" ) {
           kinematics.min_speed_theta_ = parameter.as_double(   );
           kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
           } else if (  name == plugin_name_ + ".acc_lim_x" ) {
           kinematics.acc_lim_x_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".acc_lim_y" ) {
           kinematics.acc_lim_y_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".acc_lim_theta" ) {
           kinematics.acc_lim_theta_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".decel_lim_x" ) {
           kinematics.decel_lim_x_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".decel_lim_y" ) {
           kinematics.decel_lim_y_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".decel_lim_theta" ) {
           kinematics.decel_lim_theta_ = parameter.as_double(   );
           }
           }
           }
           update_kinematics(  kinematics );
           result.successful = true;
           return result;
          }
          
     220  void KinematicsHandler::update_kinematics(  KinematicParameters kinematics )
          {
           delete kinematics_.load(   );
           kinematics_.store(  new KinematicParameters(  kinematics ) );
          }
          
          } // namespace dwb_plugins

navigation2/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_plugins/limited_accel_generator.hpp"
          #include <vector>
          #include <memory>
          #include <string>
          #include "nav_2d_utils/parameters.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace dwb_plugins
          {
          
      47  void LimitedAccelGenerator::initialize(  
      48   const nav2_util::LifecycleNode::SharedPtr & nh,  
      49   const std::string & plugin_name )
          {
           plugin_name_ = plugin_name;
           StandardTrajectoryGenerator::initialize(  nh,   plugin_name_ );
          
           try {
           nav2_util::declare_parameter_if_not_declared(  
           nh,   plugin_name + ".sim_period",   rclcpp::PARAMETER_DOUBLE );
           if (  !nh->get_parameter(  plugin_name + ".sim_period",   acceleration_time_ ) ) {
           // This actually should never appear,   since declare_parameter_if_not_declared(   )
           // completed w/o exceptions guarantee that static parameter will be initialized
           // with some value. However for reliability we should also process the case
           // when get_parameter(   ) will return a failure for some other reasons.
           throw std::runtime_error(  "Failed to get 'sim_period' value" );
           }
           } catch (  std::exception & ) {
           RCLCPP_WARN(  
           rclcpp::get_logger(  "LimitedAccelGenerator" ),  
           "'sim_period' parameter is not set for %s",   plugin_name.c_str(   ) );
           double controller_frequency = nav_2d_utils::searchAndGetParam(  
           nh,   "controller_frequency",   20.0 );
           if (  controller_frequency > 0 ) {
           acceleration_time_ = 1.0 / controller_frequency;
           } else {
           RCLCPP_WARN(  
           rclcpp::get_logger(  "LimitedAccelGenerator" ),  
           "A controller_frequency less than or equal to 0 has been set. "
           "Ignoring the parameter,   assuming a rate of 20Hz" );
           acceleration_time_ = 0.05;
           }
           }
          }
          
      82  void LimitedAccelGenerator::startNewIteration(  const nav_2d_msgs::msg::Twist2D & current_velocity )
          {
           // Limit our search space to just those within the limited acceleration_time
           velocity_iterator_->startNewIteration(  current_velocity,   acceleration_time_ );
          }
          
      88  nav_2d_msgs::msg::Twist2D LimitedAccelGenerator::computeNewVelocity(  
      89   const nav_2d_msgs::msg::Twist2D & cmd_vel,  
      90   const nav_2d_msgs::msg::Twist2D & /*start_vel*/,  
           const double /*dt*/ )
          {
           return cmd_vel;
          }
          
          } // namespace dwb_plugins
          
      98  PLUGINLIB_EXPORT_CLASS(  dwb_plugins::LimitedAccelGenerator,   dwb_core::TrajectoryGenerator )

navigation2/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_plugins/standard_traj_generator.hpp"
          #include <string>
          #include <vector>
          #include <algorithm>
          #include <memory>
          #include "dwb_plugins/xy_theta_iterator.hpp"
          #include "nav_2d_utils/parameters.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace dwb_plugins
          {
          
      49  void StandardTrajectoryGenerator::initialize(  
      50   const nav2_util::LifecycleNode::SharedPtr & nh,  
      51   const std::string & plugin_name )
          {
           plugin_name_ = plugin_name;
           kinematics_handler_ = std::make_shared<KinematicsHandler>(   );
           kinematics_handler_->initialize(  nh,   plugin_name_ );
           initializeIterator(  nh );
          
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".sim_time",   rclcpp::ParameterValue(  1.7 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".discretize_by_time",   rclcpp::ParameterValue(  false ) );
          
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".time_granularity",   rclcpp::ParameterValue(  0.5 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".linear_granularity",   rclcpp::ParameterValue(  0.5 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".angular_granularity",   rclcpp::ParameterValue(  0.025 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".include_last_point",   rclcpp::ParameterValue(  true ) );
          
           /*
           * If discretize_by_time,   then sim_granularity represents the amount of time that should be between
           * two successive points on the trajectory.
           *
           * If discretize_by_time is false,   then sim_granularity is the maximum amount of distance between
           * two successive points on the trajectory,   and angular_sim_granularity is the maximum amount of
           * angular distance between two successive points.
           */
           nh->get_parameter(  plugin_name + ".sim_time",   sim_time_ );
           nh->get_parameter(  plugin_name + ".discretize_by_time",   discretize_by_time_ );
           nh->get_parameter(  plugin_name + ".time_granularity",   time_granularity_ );
           nh->get_parameter(  plugin_name + ".linear_granularity",   linear_granularity_ );
           nh->get_parameter(  plugin_name + ".angular_granularity",   angular_granularity_ );
           nh->get_parameter(  plugin_name + ".include_last_point",   include_last_point_ );
          }
          
      94  void StandardTrajectoryGenerator::initializeIterator(  
      95   const nav2_util::LifecycleNode::SharedPtr & nh )
          {
           velocity_iterator_ = std::make_shared<XYThetaIterator>(   );
           velocity_iterator_->initialize(  nh,   kinematics_handler_,   plugin_name_ );
          }
          
     101  void StandardTrajectoryGenerator::startNewIteration(  
     102   const nav_2d_msgs::msg::Twist2D & current_velocity )
          {
           velocity_iterator_->startNewIteration(  current_velocity,   sim_time_ );
          }
          
     107  bool StandardTrajectoryGenerator::hasMoreTwists(   )
          {
           return velocity_iterator_->hasMoreTwists(   );
          }
          
     112  nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::nextTwist(   )
          {
           return velocity_iterator_->nextTwist(   );
          }
          
     117  std::vector<double> StandardTrajectoryGenerator::getTimeSteps(  
     118   const nav_2d_msgs::msg::Twist2D & cmd_vel )
          {
           std::vector<double> steps;
           if (  discretize_by_time_ ) {
           steps.resize(  ceil(  sim_time_ / time_granularity_ ) );
           } else { // discretize by distance
           double vmag = hypot(  cmd_vel.x,   cmd_vel.y );
          
           // the distance the robot would travel in sim_time if it did not change velocity
           double projected_linear_distance = vmag * sim_time_;
          
           // the angle the robot would rotate in sim_time
           double projected_angular_distance = fabs(  cmd_vel.theta ) * sim_time_;
          
           // Pick the maximum of the two
           int num_steps = ceil(  
           std::max(  
           projected_linear_distance / linear_granularity_,  
           projected_angular_distance / angular_granularity_ ) );
           steps.resize(  num_steps );
           }
           if (  steps.size(   ) == 0 ) {
           steps.resize(  1 );
           }
           std::fill(  steps.begin(   ),   steps.end(   ),   sim_time_ / steps.size(   ) );
           return steps;
          }
          
     146  dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(  
     147   const geometry_msgs::msg::Pose2D & start_pose,  
     148   const nav_2d_msgs::msg::Twist2D & start_vel,  
     149   const nav_2d_msgs::msg::Twist2D & cmd_vel )
          {
           dwb_msgs::msg::Trajectory2D traj;
           traj.velocity = cmd_vel;
           // simulate the trajectory
           geometry_msgs::msg::Pose2D pose = start_pose;
           nav_2d_msgs::msg::Twist2D vel = start_vel;
           double running_time = 0.0;
           std::vector<double> steps = getTimeSteps(  cmd_vel );
           traj.poses.push_back(  start_pose );
           for (  double dt : steps ) {
           // calculate velocities
           vel = computeNewVelocity(  cmd_vel,   vel,   dt );
          
           // update the position of the robot using the velocities passed in
           pose = computeNewPosition(  pose,   vel,   dt );
          
           traj.poses.push_back(  pose );
           traj.time_offsets.push_back(  rclcpp::Duration::from_seconds(  running_time ) );
           running_time += dt;
           } // end for simulation steps
          
           if (  include_last_point_ ) {
           traj.poses.push_back(  pose );
           traj.time_offsets.push_back(  rclcpp::Duration::from_seconds(  running_time ) );
           }
          
           return traj;
          }
          
          /**
           * change vel using acceleration limits to converge towards sample_target-vel
           */
     182  nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity(  
     183   const nav_2d_msgs::msg::Twist2D & cmd_vel,  
     184   const nav_2d_msgs::msg::Twist2D & start_vel,   const double dt )
          {
           KinematicParameters kinematics = kinematics_handler_->getKinematics(   );
           nav_2d_msgs::msg::Twist2D new_vel;
           new_vel.x = projectVelocity(  
           start_vel.x,   kinematics.getAccX(   ),  
           kinematics.getDecelX(   ),   dt,   cmd_vel.x );
           new_vel.y = projectVelocity(  
           start_vel.y,   kinematics.getAccY(   ),  
           kinematics.getDecelY(   ),   dt,   cmd_vel.y );
           new_vel.theta = projectVelocity(  
           start_vel.theta,  
           kinematics.getAccTheta(   ),   kinematics.getDecelTheta(   ),  
           dt,   cmd_vel.theta );
           return new_vel;
          }
          
     201  geometry_msgs::msg::Pose2D StandardTrajectoryGenerator::computeNewPosition(  
     202   const geometry_msgs::msg::Pose2D start_pose,  
     203   const nav_2d_msgs::msg::Twist2D & vel,   const double dt )
          {
           geometry_msgs::msg::Pose2D new_pose;
           new_pose.x = start_pose.x +
           (  vel.x * cos(  start_pose.theta ) + vel.y * cos(  M_PI_2 + start_pose.theta ) ) * dt;
           new_pose.y = start_pose.y +
           (  vel.x * sin(  start_pose.theta ) + vel.y * sin(  M_PI_2 + start_pose.theta ) ) * dt;
           new_pose.theta = start_pose.theta + vel.theta * dt;
           return new_pose;
          }
          
          } // namespace dwb_plugins
          
     216  PLUGINLIB_EXPORT_CLASS(  
           dwb_plugins::StandardTrajectoryGenerator,  
           dwb_core::TrajectoryGenerator )

navigation2/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "dwb_plugins/xy_theta_iterator.hpp"
          
          #include <cmath>
          #include <memory>
          #include <string>
          
          #include "nav_2d_utils/parameters.hpp"
          #include "nav2_util/node_utils.hpp"
          
          #define EPSILON 1E-5
          
          namespace dwb_plugins
          {
      48  void XYThetaIterator::initialize(  
      49   const nav2_util::LifecycleNode::SharedPtr & nh,  
      50   KinematicsHandler::Ptr kinematics,  
      51   const std::string & plugin_name )
          {
           kinematics_handler_ = kinematics;
          
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".vx_samples",   rclcpp::ParameterValue(  20 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".vy_samples",   rclcpp::ParameterValue(  5 ) );
           nav2_util::declare_parameter_if_not_declared(  
           nh,  
           plugin_name + ".vtheta_samples",   rclcpp::ParameterValue(  20 ) );
          
           nh->get_parameter(  plugin_name + ".vx_samples",   vx_samples_ );
           nh->get_parameter(  plugin_name + ".vy_samples",   vy_samples_ );
           nh->get_parameter(  plugin_name + ".vtheta_samples",   vtheta_samples_ );
          }
          
      70  void XYThetaIterator::startNewIteration(  
      71   const nav_2d_msgs::msg::Twist2D & current_velocity,  
           double dt )
          {
           KinematicParameters kinematics = kinematics_handler_->getKinematics(   );
           x_it_ = std::make_shared<OneDVelocityIterator>(  
           current_velocity.x,  
           kinematics.getMinX(   ),   kinematics.getMaxX(   ),  
           kinematics.getAccX(   ),   kinematics.getDecelX(   ),  
           dt,   vx_samples_ );
           y_it_ = std::make_shared<OneDVelocityIterator>(  
           current_velocity.y,  
           kinematics.getMinY(   ),   kinematics.getMaxY(   ),  
           kinematics.getAccY(   ),   kinematics.getDecelY(   ),  
           dt,   vy_samples_ );
           th_it_ = std::make_shared<OneDVelocityIterator>(  
           current_velocity.theta,  
           kinematics.getMinTheta(   ),   kinematics.getMaxTheta(   ),  
           kinematics.getAccTheta(   ),   kinematics.getDecelTheta(   ),  
           dt,   vtheta_samples_ );
           if (  !isValidVelocity(   ) ) {
           iterateToValidVelocity(   );
           }
          }
          
      95  bool XYThetaIterator::isValidSpeed(  double x,   double y,   double theta )
          {
           KinematicParameters kinematics = kinematics_handler_->getKinematics(   );
           double vmag_sq = x * x + y * y;
           if (  kinematics.getMaxSpeedXY(   ) >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ(   ) + EPSILON ) {
           return false;
           }
           if (  kinematics.getMinSpeedXY(   ) >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ(   ) &&
           kinematics.getMinSpeedTheta(   ) >= 0.0 && fabs(  theta ) + EPSILON < kinematics.getMinSpeedTheta(   ) )
           {
           return false;
           }
           if (  vmag_sq == 0.0 && th_it_->getVelocity(   ) == 0.0 ) {
           return false;
           }
           return true;
          }
          
     113  bool XYThetaIterator::isValidVelocity(   )
          {
           return isValidSpeed(  
           x_it_->getVelocity(   ),   y_it_->getVelocity(   ),  
           th_it_->getVelocity(   ) );
          }
          
     120  bool XYThetaIterator::hasMoreTwists(   )
          {
           return x_it_ && !x_it_->isFinished(   );
          }
          
     125  nav_2d_msgs::msg::Twist2D XYThetaIterator::nextTwist(   )
          {
           nav_2d_msgs::msg::Twist2D velocity;
           velocity.x = x_it_->getVelocity(   );
           velocity.y = y_it_->getVelocity(   );
           velocity.theta = th_it_->getVelocity(   );
          
           iterateToValidVelocity(   );
          
           return velocity;
          }
          
     137  void XYThetaIterator::iterateToValidVelocity(   )
          {
           bool valid = false;
           while (  !valid && hasMoreTwists(   ) ) {
           ++(  *th_it_ );
           if (  th_it_->isFinished(   ) ) {
           th_it_->reset(   );
           ++(  *y_it_ );
           if (  y_it_->isFinished(   ) ) {
           y_it_->reset(   );
           ++(  *x_it_ );
           }
           }
           valid = isValidVelocity(   );
           }
          }
          
          } // namespace dwb_plugins

navigation2/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <string>
          #include <memory>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "dwb_plugins/kinematic_parameters.hpp"
          
          using rcl_interfaces::msg::Parameter;
          using rcl_interfaces::msg::ParameterType;
          using rcl_interfaces::msg::ParameterEvent;
          
      46  class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
          {
          public:
      49   void simulate_event(  
      50   std::vector<rclcpp::Parameter> parameters )
           {
           dynamicParametersCallback(  parameters );
           }
          };
          
      56  TEST(  KinematicParameters,   SetAllParameters ) {
           std::string nodeName = "test_node";
           auto node = nav2_util::LifecycleNode::make_shared(  nodeName );
           KinematicsHandlerTest kh;
           kh.initialize(  node,   nodeName );
          
           std::vector<rclcpp::Parameter> parameters;
           rclcpp::Parameter
           p_minX(  nodeName + ".min_vel_x",   12.34 ),  
           p_maxX(  nodeName + ".max_vel_x",   23.45 ),  
           p_minY(  nodeName + ".min_vel_y",   34.56 ),  
           p_maxY(  nodeName + ".max_vel_y",   45.67 ),  
           p_accX(  nodeName + ".acc_lim_x",   56.78 ),  
           p_decelX(  nodeName + ".acc_lim_y",   67.89 ),  
           p_accY(  nodeName + ".decel_lim_x",   78.90 ),  
           p_decelY(  nodeName + ".decel_lim_y",   89.01 ),  
           p_minSpeedXY(  nodeName + ".min_speed_xy",   90.12 ),  
           p_maxSpeedXY(  nodeName + ".max_speed_xy",   123.456 ),  
           p_maxTheta(  nodeName + ".max_vel_theta",   345.678 ),  
           p_accTheta(  nodeName + ".acc_lim_theta",   234.567 ),  
           p_decelTheta(  nodeName + ".decel_lim_theta",   456.789 ),  
           p_minSpeedTheta(  nodeName + ".min_speed_theta",   567.890 );
          
           parameters.push_back(  p_minX );
           parameters.push_back(  p_minX );
           parameters.push_back(  p_maxX );
           parameters.push_back(  p_minY );
           parameters.push_back(  p_maxY );
           parameters.push_back(  p_accX );
           parameters.push_back(  p_accY );
           parameters.push_back(  p_decelX );
           parameters.push_back(  p_decelY );
           parameters.push_back(  p_minSpeedXY );
           parameters.push_back(  p_maxSpeedXY );
           parameters.push_back(  p_maxTheta );
           parameters.push_back(  p_accTheta );
           parameters.push_back(  p_decelTheta );
           parameters.push_back(  p_minSpeedTheta );
          
           kh.simulate_event(  parameters );
          
           dwb_plugins::KinematicParameters kp = kh.getKinematics(   );
          
           EXPECT_EQ(  kp.getMinX(   ),   12.34 );
           EXPECT_EQ(  kp.getMaxX(   ),   23.45 );
           EXPECT_EQ(  kp.getMinY(   ),   34.56 );
           EXPECT_EQ(  kp.getMaxY(   ),   45.67 );
           EXPECT_EQ(  kp.getAccX(   ),   56.78 );
           EXPECT_EQ(  kp.getAccY(   ),   67.89 );
           EXPECT_EQ(  kp.getDecelX(   ),   78.90 );
           EXPECT_EQ(  kp.getDecelY(   ),   89.01 );
           EXPECT_EQ(  kp.getMinSpeedXY(   ),   90.12 );
           EXPECT_EQ(  kp.getMaxSpeedXY(   ),   123.456 );
           EXPECT_EQ(  kp.getAccTheta(   ),   234.567 );
           EXPECT_EQ(  kp.getMaxTheta(   ),   345.678 );
           EXPECT_EQ(  kp.getDecelTheta(   ),   456.789 );
           EXPECT_EQ(  kp.getMinSpeedTheta(   ),   567.890 );
          }
          
          
     116  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2020,   Samsung Research Russia
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Alexey Merzlyakov
           */
          
          #include <gtest/gtest.h>
          
          #include <string>
          #include <memory>
          #include <chrono>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          #include "dwb_plugins/kinematic_parameters.hpp"
          
          using namespace std::chrono_literals;
          
      51  static constexpr double EPSILON = 1e-5;
          
          static const char NODE_NAME[] = "test_node";
          static const double MAX_VEL_X = 40.0;
          static const double MAX_VEL_Y = 30.0;
          static const double MAX_VEL_THETA = 15.0;
          static const double MAX_VEL_LINEAR = 50.0;
          
      59  class TestNode : public ::testing::Test
          {
          public:
      62   TestNode(   )
           {
           const std::string node_name = NODE_NAME;
           node_ = nav2_util::LifecycleNode::make_shared(  node_name );
          
           node_->declare_parameter(  
           node_name + ".max_vel_x",   rclcpp::ParameterValue(  MAX_VEL_X ) );
           node_->set_parameter(  
           rclcpp::Parameter(  node_name + ".max_vel_x",   MAX_VEL_X ) );
          
           node_->declare_parameter(  
           node_name + ".max_vel_y",   rclcpp::ParameterValue(  MAX_VEL_Y ) );
           node_->set_parameter(  
           rclcpp::Parameter(  node_name + ".max_vel_y",   MAX_VEL_Y ) );
          
           node_->declare_parameter(  
           node_name + ".max_vel_theta",   rclcpp::ParameterValue(  MAX_VEL_THETA ) );
           node_->set_parameter(  
           rclcpp::Parameter(  node_name + ".max_vel_theta",   MAX_VEL_THETA ) );
          
           node_->declare_parameter(  
           node_name + ".max_speed_xy",   rclcpp::ParameterValue(  MAX_VEL_LINEAR ) );
           node_->set_parameter(  
           rclcpp::Parameter(  node_name + ".max_speed_xy",   MAX_VEL_LINEAR ) );
           }
          
      88   ~TestNode(   ) {}
          
          protected:
      91   nav2_util::LifecycleNode::SharedPtr node_;
          };
          
      94  TEST_F(  TestNode,   TestPercentLimit )
          {
           dwb_plugins::KinematicsHandler kh;
           kh.initialize(  node_,   NODE_NAME );
          
           dwb_plugins::KinematicParameters kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   MAX_VEL_LINEAR,   EPSILON );
          
           // Set speed limit 30% from maximum robot speed
           kh.setSpeedLimit(  30,   true );
          
           // Update KinematicParameters values from KinematicsHandler
           kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X * 0.3,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y * 0.3,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA * 0.3,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   MAX_VEL_LINEAR * 0.3,   EPSILON );
          
           // Restore maximum speed to its default
           kh.setSpeedLimit(  nav2_costmap_2d::NO_SPEED_LIMIT,   true );
          
           // Update KinematicParameters values from KinematicsHandler
           kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   MAX_VEL_LINEAR,   EPSILON );
          }
          
     126  TEST_F(  TestNode,   TestAbsoluteLimit )
          {
           dwb_plugins::KinematicsHandler kh;
           kh.initialize(  node_,   NODE_NAME );
          
           dwb_plugins::KinematicParameters kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   MAX_VEL_LINEAR,   EPSILON );
          
           // Set speed limit 35.0 m/s
           kh.setSpeedLimit(  35.0,   false );
          
           // Update KinematicParameters values from KinematicsHandler
           kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X * 35.0 / MAX_VEL_LINEAR,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y * 35.0 / MAX_VEL_LINEAR,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA * 35.0 / MAX_VEL_LINEAR,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   35.0,   EPSILON );
          
           // Restore maximum speed to its default
           kh.setSpeedLimit(  nav2_costmap_2d::NO_SPEED_LIMIT,   false );
          
           // Update KinematicParameters values from KinematicsHandler
           kp = kh.getKinematics(   );
           EXPECT_NEAR(  kp.getMaxX(   ),   MAX_VEL_X,   EPSILON );
           EXPECT_NEAR(  kp.getMaxY(   ),   MAX_VEL_Y,   EPSILON );
           EXPECT_NEAR(  kp.getMaxTheta(   ),   MAX_VEL_THETA,   EPSILON );
           EXPECT_NEAR(  kp.getMaxSpeedXY(   ),   MAX_VEL_LINEAR,   EPSILON );
          }
          
     158  int main(  int argc,   char ** argv )
          {
           // Initialize the system
           testing::InitGoogleTest(  &argc,   argv );
           rclcpp::init(  argc,   argv );
          
           // Actual testing
           bool test_result = RUN_ALL_TESTS(   );
          
           // Shutdown
           rclcpp::shutdown(   );
          
           return test_result;
          }

navigation2/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <cmath>
          #include <vector>
          #include <algorithm>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "dwb_plugins/standard_traj_generator.hpp"
          #include "dwb_plugins/limited_accel_generator.hpp"
          #include "dwb_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          
          using std::hypot;
          using std::fabs;
          using dwb_plugins::StandardTrajectoryGenerator;
          
          geometry_msgs::msg::Pose2D origin;
          nav_2d_msgs::msg::Twist2D zero;
          nav_2d_msgs::msg::Twist2D forward;
          
      54  class LimitedAccelGeneratorTest : public dwb_plugins::LimitedAccelGenerator
          {
          public:
      57   double getAccelerationTime(   )
           {
           return acceleration_time_;
           }
          };
          
      63  std::vector<rclcpp::Parameter> getDefaultKinematicParameters(   )
          {
           std::vector<rclcpp::Parameter> parameters;
           parameters.push_back(  rclcpp::Parameter(  "dwb.min_vel_x",   0.0 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.max_vel_x",   0.55 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.min_vel_y",   -0.1 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.max_vel_y",   0.1 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.max_vel_theta",   1.0 ) );
          
           parameters.push_back(  rclcpp::Parameter(  "dwb.acc_lim_x",   2.5 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.acc_lim_y",   2.5 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.acc_lim_theta",   3.2 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.decel_lim_x",   -2.5 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.decel_lim_y",   -2.5 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.decel_lim_theta",   -3.2 ) );
          
           parameters.push_back(  rclcpp::Parameter(  "dwb.min_speed_xy",   0.1 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.max_speed_xy",   0.55 ) );
           parameters.push_back(  rclcpp::Parameter(  "dwb.min_speed_theta",   0.4 ) );
          
           return parameters;
          }
          
      86  rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(  
           const std::string & name,  
           const std::vector<rclcpp::Parameter> & overrides = {} )
          {
           rclcpp::NodeOptions node_options;
           node_options.parameter_overrides(  getDefaultKinematicParameters(   ) );
           node_options.parameter_overrides(   ).insert(  
           node_options.parameter_overrides(   ).end(   ),   overrides.begin(   ),   overrides.end(   ) );
          
           auto node = rclcpp_lifecycle::LifecycleNode::make_shared(  name,   node_options );
           node->on_configure(  node->get_current_state(   ) );
           node->on_activate(  node->get_current_state(   ) );
          
           return node;
          }
          
     102  void checkLimits(  
           const std::vector<nav_2d_msgs::msg::Twist2D> & twists,  
           double exp_min_x,   double exp_max_x,   double exp_min_y,   double exp_max_y,  
           double exp_min_theta,   double exp_max_theta,  
           double exp_max_xy = -1.0,  
           double exp_min_xy = -1.0,   double exp_min_speed_theta = -1.0 )
          {
           ASSERT_GT(  twists.size(   ),   0u );
           nav_2d_msgs::msg::Twist2D first = twists[0];
          
           double min_x = first.x,   max_x = first.x,   min_y = first.y,   max_y = first.y;
           double min_theta = first.theta,   max_theta = first.theta;
           double max_xy = hypot(  first.x,   first.y );
          
           for (  nav_2d_msgs::msg::Twist2D twist : twists ) {
           min_x = std::min(  min_x,   twist.x );
           min_y = std::min(  min_y,   twist.y );
           min_theta = std::min(  min_theta,   twist.theta );
           max_x = std::max(  max_x,   twist.x );
           max_y = std::max(  max_y,   twist.y );
           max_theta = std::max(  max_theta,   twist.theta );
           double hyp = hypot(  twist.x,   twist.y );
           max_xy = std::max(  max_xy,   hyp );
          
           if (  exp_min_xy >= 0 && exp_min_speed_theta >= 0 ) {
           EXPECT_TRUE(  fabs(  twist.theta ) >= exp_min_speed_theta || hyp >= exp_min_xy );
           }
           }
           EXPECT_DOUBLE_EQ(  min_x,   exp_min_x );
           EXPECT_DOUBLE_EQ(  max_x,   exp_max_x );
           EXPECT_DOUBLE_EQ(  min_y,   exp_min_y );
           EXPECT_DOUBLE_EQ(  max_y,   exp_max_y );
           EXPECT_DOUBLE_EQ(  min_theta,   exp_min_theta );
           EXPECT_DOUBLE_EQ(  max_theta,   exp_max_theta );
           if (  exp_max_xy >= 0 ) {
           EXPECT_DOUBLE_EQ(  max_xy,   exp_max_xy );
           }
          }
          
     141  double durationToSec(  builtin_interfaces::msg::Duration d )
          {
           return d.sec + d.nanosec * 1e-9;
          }
          
     146  TEST(  VelocityIterator,   standard_gen )
          {
           auto nh = makeTestNode(  "st_gen" );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           EXPECT_EQ(  twists.size(   ),   1926u );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0,   0.55,   0.1,   0.4 );
          }
          
     156  TEST(  VelocityIterator,   max_xy )
          {
           auto nh = makeTestNode(  "max_xy",   {rclcpp::Parameter(  "dwb.max_speed_xy",   1.0 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
          
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           // Expect more twists since max_speed_xy is now beyond feasible limits
           EXPECT_EQ(  twists.size(   ),   2010u );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0,   hypot(  0.55,   0.1 ) );
          }
          
     168  TEST(  VelocityIterator,   min_xy )
          {
           auto nh = makeTestNode(  "min_xy",   {rclcpp::Parameter(  "dwb.min_speed_xy",   -1.0 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           // Expect even more since theres no min_speed_xy
           EXPECT_EQ(  twists.size(   ),   2015u );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0 );
          }
          
     179  TEST(  VelocityIterator,   min_theta )
          {
           auto nh = makeTestNode(  "min_theta",   {rclcpp::Parameter(  "dwb.min_speed_theta",   -1.0 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           // Expect even more since theres no min_speed_xy
           EXPECT_EQ(  twists.size(   ),   2015u );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0 );
          }
          
     190  TEST(  VelocityIterator,   no_limits )
          {
           auto nh = makeTestNode(  
           "no_limits",   {
           rclcpp::Parameter(  "dwb.max_speed_xy",   -1.0 ),  
           rclcpp::Parameter(  "dwb.min_speed_xy",   -1.0 ),  
           rclcpp::Parameter(  "dwb.min_speed_theta",   -1.0 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           // vx_samples * vtheta_samples * vy_samples + added zero theta samples - (  0,  0,  0 )
           EXPECT_EQ(  twists.size(   ),   20u * 20u * 5u + 100u - 1u );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0,   hypot(  0.55,   0.1 ),   0.0,   0.0 );
          }
          
     205  TEST(  VelocityIterator,   no_limits_samples )
          {
           const int x_samples = 10,   y_samples = 3,   theta_samples = 5;
           auto nh = makeTestNode(  
           "no_limits_samples",   {
           rclcpp::Parameter(  "dwb.max_speed_xy",   -1.0 ),  
           rclcpp::Parameter(  "dwb.min_speed_xy",   -1.0 ),  
           rclcpp::Parameter(  "dwb.min_speed_theta",   -1.0 ),  
           rclcpp::Parameter(  "dwb.vx_samples",   x_samples ),  
           rclcpp::Parameter(  "dwb.vy_samples",   y_samples ),  
           rclcpp::Parameter(  "dwb.vtheta_samples",   theta_samples )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           EXPECT_EQ(  twists.size(   ),   static_cast<unsigned>(  x_samples * y_samples * theta_samples - 1 ) );
           checkLimits(  twists,   0.0,   0.55,   -0.1,   0.1,   -1.0,   1.0,   hypot(  0.55,   0.1 ),   0.0,   0.0 );
          }
          
     223  TEST(  VelocityIterator,   dwa_gen )
          {
           auto nh = makeTestNode(  "dwa_gen",   {rclcpp::Parameter(  "dwb.min_speed_theta",   -1.0 )} );
           dwb_plugins::LimitedAccelGenerator gen;
           gen.initialize(  nh,   "dwb" );
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  zero );
           // Same as no-limits since everything is within our velocity limits
           EXPECT_EQ(  twists.size(   ),   20u * 20u * 5u + 100u - 1u );
           checkLimits(  twists,   0.0,   0.125,   -0.1,   0.1,   -0.16,   0.16,   hypot(  0.125,   0.1 ),   0.0,   0.1 );
          }
          
     234  TEST(  VelocityIterator,   dwa_gen_zero_frequency )
          {
           auto nh = makeTestNode(  "dwa_gen" );
           nh->declare_parameter(  "controller_frequency",   0.0 );
           LimitedAccelGeneratorTest gen;
           gen.initialize(  nh,   "dwb" );
           // Default value should be 0.05
           EXPECT_EQ(  gen.getAccelerationTime(   ),   0.05 );
          }
          
     244  TEST(  VelocityIterator,   dwa_gen_one_frequency )
          {
           auto nh = makeTestNode(  "dwa_gen" );
           nh->declare_parameter(  "controller_frequency",   1.0 );
           LimitedAccelGeneratorTest gen;
           gen.initialize(  nh,   "dwb" );
           EXPECT_EQ(  gen.getAccelerationTime(   ),   1.0 );
          }
          
     253  TEST(  VelocityIterator,   dwa_gen_ten_frequency )
          {
           auto nh = makeTestNode(  "dwa_gen" );
           nh->declare_parameter(  "controller_frequency",   10.0 );
           LimitedAccelGeneratorTest gen;
           gen.initialize(  nh,   "dwb" );
           EXPECT_EQ(  gen.getAccelerationTime(   ),   0.1 );
          }
          
     262  TEST(  VelocityIterator,   dwa_gen_fifty_frequency )
          {
           auto nh = makeTestNode(  "dwa_gen" );
           nh->declare_parameter(  "controller_frequency",   50.0 );
           LimitedAccelGeneratorTest gen;
           gen.initialize(  nh,   "dwb" );
           EXPECT_EQ(  gen.getAccelerationTime(   ),   0.02 );
          }
          
     271  TEST(  VelocityIterator,   dwa_gen_hundred_frequency )
          {
           auto nh = makeTestNode(  "dwa_gen" );
           nh->declare_parameter(  "controller_frequency",   100.0 );
           LimitedAccelGeneratorTest gen;
           gen.initialize(  nh,   "dwb" );
           EXPECT_EQ(  gen.getAccelerationTime(   ),   0.01 );
          }
          
     280  TEST(  VelocityIterator,   nonzero )
          {
           auto nh = makeTestNode(  "nonzero",   {rclcpp::Parameter(  "dwb.min_speed_theta",   -1.0 )} );
           dwb_plugins::LimitedAccelGenerator gen;
           gen.initialize(  nh,   "dwb" );
           nav_2d_msgs::msg::Twist2D initial;
           initial.x = 0.1;
           initial.y = -0.08;
           initial.theta = 0.05;
           std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(  initial );
           EXPECT_EQ(  twists.size(   ),   2519u );
           checkLimits(  
           twists,   0.0,   0.225,   -0.1,   0.045,   -0.11000000000000003,   0.21,  
           0.24622144504490268,   0.0,   0.1 );
          }
          
     296  void matchPose(  const geometry_msgs::msg::Pose2D & a,   const geometry_msgs::msg::Pose2D & b )
          {
           EXPECT_DOUBLE_EQ(  a.x,   b.x );
           EXPECT_DOUBLE_EQ(  a.y,   b.y );
           EXPECT_DOUBLE_EQ(  a.theta,   b.theta );
          }
          
     303  void matchPose(  
           const geometry_msgs::msg::Pose2D & a,   const double x,   const double y,  
           const double theta )
          {
           EXPECT_DOUBLE_EQ(  a.x,   x );
           EXPECT_DOUBLE_EQ(  a.y,   y );
           EXPECT_DOUBLE_EQ(  a.theta,   theta );
          }
          
     312  void matchTwist(  const nav_2d_msgs::msg::Twist2D & a,   const nav_2d_msgs::msg::Twist2D & b )
          {
           EXPECT_DOUBLE_EQ(  a.x,   b.x );
           EXPECT_DOUBLE_EQ(  a.y,   b.y );
           EXPECT_DOUBLE_EQ(  a.theta,   b.theta );
          }
          
     319  void matchTwist(  
           const nav_2d_msgs::msg::Twist2D & a,   const double x,   const double y,  
           const double theta )
          {
           EXPECT_DOUBLE_EQ(  a.x,   x );
           EXPECT_DOUBLE_EQ(  a.y,   y );
           EXPECT_DOUBLE_EQ(  a.theta,   theta );
          }
          
          const double DEFAULT_SIM_TIME = 1.7;
          
     330  TEST(  TrajectoryGenerator,   basic )
          {
           auto nh = makeTestNode(  "basic",   {rclcpp::Parameter(  "dwb.linear_granularity",   0.5 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   forward,   forward );
           matchTwist(  res.velocity,   forward );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   DEFAULT_SIM_TIME );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   4 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[n - 1],   DEFAULT_SIM_TIME * forward.x,   0,   0 );
          }
          
     346  TEST(  TrajectoryGenerator,   basic_no_last_point )
          {
           auto nh = makeTestNode(  
           "basic_no_last_point",   {
           rclcpp::Parameter(  "dwb.include_last_point",   false ),  
           rclcpp::Parameter(  "dwb.linear_granularity",   0.5 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   forward,   forward );
           matchTwist(  res.velocity,   forward );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   DEFAULT_SIM_TIME / 2 );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   3 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[n - 2],   0.255,   0,   0 );
          }
          
     365  TEST(  TrajectoryGenerator,   too_slow )
          {
           auto nh = makeTestNode(  "too_slow",   {rclcpp::Parameter(  "dwb.linear_granularity",   0.5 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           nav_2d_msgs::msg::Twist2D cmd;
           cmd.x = 0.2;
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   cmd,   cmd );
           matchTwist(  res.velocity,   cmd );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   DEFAULT_SIM_TIME );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   3 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
          }
          
     382  TEST(  TrajectoryGenerator,   holonomic )
          {
           auto nh = makeTestNode(  "holonomic",   {rclcpp::Parameter(  "dwb.linear_granularity",   0.5 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           nav_2d_msgs::msg::Twist2D cmd;
           cmd.x = 0.3;
           cmd.y = 0.2;
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   cmd,   cmd );
           matchTwist(  res.velocity,   cmd );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   DEFAULT_SIM_TIME );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   4 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[n - 1],   cmd.x * DEFAULT_SIM_TIME,   cmd.y * DEFAULT_SIM_TIME,   0 );
          }
          
     401  TEST(  TrajectoryGenerator,   twisty )
          {
           auto nh = makeTestNode(  
           "twisty",   {
           rclcpp::Parameter(  "dwb.linear_granularity",   0.5 ),  
           rclcpp::Parameter(  "dwb.angular_granularity",   0.025 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           nav_2d_msgs::msg::Twist2D cmd;
           cmd.x = 0.3;
           cmd.y = -0.2;
           cmd.theta = 0.111;
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   cmd,   cmd );
           matchTwist(  res.velocity,   cmd );
           EXPECT_NEAR(  durationToSec(  res.time_offsets.back(   ) ),   DEFAULT_SIM_TIME,   1.0E-5 );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   10 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
           matchPose(  
           res.poses[n - 1],   0.5355173615993063,   -0.29635287789821596,  
           cmd.theta * DEFAULT_SIM_TIME );
          }
          
     426  TEST(  TrajectoryGenerator,   sim_time )
          {
           const double sim_time = 2.5;
           auto nh = makeTestNode(  
           "sim_time",   {
           rclcpp::Parameter(  "dwb.sim_time",   sim_time ),  
           rclcpp::Parameter(  "dwb.linear_granularity",   0.5 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   forward,   forward );
           matchTwist(  res.velocity,   forward );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   sim_time );
           int n = res.poses.size(   );
           EXPECT_EQ(  n,   4 );
           ASSERT_GT(  n,   0 );
          
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[n - 2],   sim_time * forward.x,   0,   0 );
          }
          
     446  TEST(  TrajectoryGenerator,   accel )
          {
           auto nh = makeTestNode(  
           "accel",   {
           rclcpp::Parameter(  "dwb.sim_time",   5.0 ),  
           rclcpp::Parameter(  "dwb.discretize_by_time",   true ),  
           rclcpp::Parameter(  "dwb.time_granularity",   1.0 ),  
           rclcpp::Parameter(  "dwb.acc_lim_x",   0.1 ),  
           rclcpp::Parameter(  "dwb.min_speed_xy",   -1.0 )} );
           StandardTrajectoryGenerator gen;
           gen.initialize(  nh,   "dwb" );
          
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   zero,   forward );
           matchTwist(  res.velocity,   forward );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   5.0 );
           ASSERT_EQ(  res.poses.size(   ),   7u );
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[1],   0.1,   0,   0 );
           matchPose(  res.poses[2],   0.3,   0,   0 );
           matchPose(  res.poses[3],   0.6,   0,   0 );
           matchPose(  res.poses[4],   0.9,   0,   0 );
           matchPose(  res.poses[5],   1.2,   0,   0 );
          }
          
     470  TEST(  TrajectoryGenerator,   dwa )
          {
           auto nh = makeTestNode(  
           "dwa",   {
           rclcpp::Parameter(  "dwb.sim_period",   1.0 ),  
           rclcpp::Parameter(  "dwb.sim_time",   5.0 ),  
           rclcpp::Parameter(  "dwb.discretize_by_time",   true ),  
           rclcpp::Parameter(  "dwb.time_granularity",   1.0 ),  
           rclcpp::Parameter(  "dwb.acc_lim_x",   0.1 ),  
           rclcpp::Parameter(  "dwb.min_speed_xy",   -1.0 )} );
           dwb_plugins::LimitedAccelGenerator gen;
           gen.initialize(  nh,   "dwb" );
          
           dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(  origin,   zero,   forward );
           matchTwist(  res.velocity,   forward );
           EXPECT_DOUBLE_EQ(  durationToSec(  res.time_offsets.back(   ) ),   5.0 );
           ASSERT_EQ(  res.poses.size(   ),   7u );
           matchPose(  res.poses[0],   origin );
           matchPose(  res.poses[1],   0.3,   0,   0 );
           matchPose(  res.poses[2],   0.6,   0,   0 );
           matchPose(  res.poses[3],   0.9,   0,   0 );
           matchPose(  res.poses[4],   1.2,   0,   0 );
           matchPose(  res.poses[5],   1.5,   0,   0 );
          }
          
     495  int main(  int argc,   char ** argv )
          {
           forward.x = 0.3;
           rclcpp::init(  0,   nullptr );
           testing::InitGoogleTest(  &argc,   argv );
           int ret = RUN_ALL_TESTS(   );
           rclcpp::shutdown(   );
           return ret;
          }

navigation2/nav2_dwb_controller/dwb_plugins/test/velocity_iterator_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "gtest/gtest.h"
          #include "dwb_plugins/one_d_velocity_iterator.hpp"
          
          using dwb_plugins::OneDVelocityIterator;
          
          const double EPSILON = 1e-3;
          
      42  TEST(  VelocityIterator,   basics )
          {
           OneDVelocityIterator it(  2.0,   0.0,   5.0,   1.0,   -1.0,   1.0,   2 );
           EXPECT_FALSE(  it.isFinished(   ) );
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
           EXPECT_FALSE(  it.isFinished(   ) );
           ++it;
           EXPECT_FALSE(  it.isFinished(   ) );
           EXPECT_NEAR(  it.getVelocity(   ),   3.0,   EPSILON );
           EXPECT_FALSE(  it.isFinished(   ) );
           ++it;
           EXPECT_TRUE(  it.isFinished(   ) );
           it.reset(   );
           EXPECT_FALSE(  it.isFinished(   ) );
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
          }
          
      59  TEST(  VelocityIterator,   limits )
          {
           OneDVelocityIterator it(  2.0,   1.5,   2.5,   1.0,   -1.0,   1.0,   2 );
           EXPECT_NEAR(  it.getVelocity(   ),   1.5,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.5,   EPSILON );
          }
          
      67  TEST(  VelocityIterator,   acceleration )
          {
           OneDVelocityIterator it(  2.0,   0.0,   5.0,   0.5,   -0.5,   1.0,   2 );
           EXPECT_NEAR(  it.getVelocity(   ),   1.5,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.5,   EPSILON );
          }
          
          
      76  TEST(  VelocityIterator,   time )
          {
           OneDVelocityIterator it(  2.0,   0.0,   5.0,   1.0,   -1.0,   0.5,   2 );
           EXPECT_NEAR(  it.getVelocity(   ),   1.5,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.5,   EPSILON );
          }
          
      84  TEST(  VelocityIterator,   samples )
          {
           OneDVelocityIterator it(  2.0,   0.0,   5.0,   1.0,   -1.0,   1.0,   3 );
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   3.0,   EPSILON );
           ++it;
           EXPECT_TRUE(  it.isFinished(   ) );
          }
          
          
      97  TEST(  VelocityIterator,   samples2 )
          {
           OneDVelocityIterator it(  2.0,   0.0,   5.0,   1.0,   -1.0,   1.0,   5 );
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   1.5,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   2.5,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   3.0,   EPSILON );
           ++it;
           EXPECT_TRUE(  it.isFinished(   ) );
          }
          
     113  TEST(  VelocityIterator,   around_zero )
          {
           OneDVelocityIterator it(  0.0,   -5.0,   5.0,   1.0,   -1.0,   1.0,   2 );
           EXPECT_NEAR(  it.getVelocity(   ),   -1.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   0.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
           ++it;
          }
          
     124  TEST(  VelocityIterator,   around_zero2 )
          {
           OneDVelocityIterator it(  0.0,   -5.0,   5.0,   1.0,   -1.0,   1.0,   4 );
           EXPECT_NEAR(  it.getVelocity(   ),   -1.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   -0.3333,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   0.0,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   0.3333,   EPSILON );
           ++it;
           EXPECT_NEAR(  it.getVelocity(   ),   1.0,   EPSILON );
           ++it;
          }
          
     139  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV_2D_UTILS__CONVERSIONS_HPP_
          #define NAV_2D_UTILS__CONVERSIONS_HPP_
          
          #include <vector>
          #include <string>
          #include "geometry_msgs/msg/pose.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          #include "nav_2d_msgs/msg/twist2_d.hpp"
          #include "nav_2d_msgs/msg/path2_d.hpp"
          #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "tf2/convert.h"
          
          namespace nav_2d_utils
          {
      51  geometry_msgs::msg::Twist twist2Dto3D(  const nav_2d_msgs::msg::Twist2D & cmd_vel_2d );
      52  nav_2d_msgs::msg::Twist2D twist3Dto2D(  const geometry_msgs::msg::Twist & cmd_vel );
          // nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(  const tf2::Stamped<tf2::Pose>& pose );
      54  nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(  const geometry_msgs::msg::PoseStamped & pose );
      55  geometry_msgs::msg::Pose2D poseToPose2D(  const geometry_msgs::msg::Pose & pose );
      56  geometry_msgs::msg::Pose pose2DToPose(  const geometry_msgs::msg::Pose2D & pose2d );
      57  geometry_msgs::msg::PoseStamped pose2DToPoseStamped(  
           const nav_2d_msgs::msg::Pose2DStamped & pose2d );
      59  geometry_msgs::msg::PoseStamped pose2DToPoseStamped(  
           const geometry_msgs::msg::Pose2D & pose2d,  
           const std::string & frame,   const rclcpp::Time & stamp );
      62  nav_msgs::msg::Path posesToPath(  const std::vector<geometry_msgs::msg::PoseStamped> & poses );
      63  nav_2d_msgs::msg::Path2D pathToPath2D(  const nav_msgs::msg::Path & path );
      64  nav_msgs::msg::Path poses2DToPath(  
           const std::vector<geometry_msgs::msg::Pose2D> & poses,  
           const std::string & frame,   const rclcpp::Time & stamp );
      67  nav_msgs::msg::Path pathToPath(  const nav_2d_msgs::msg::Path2D & path2d );
          
          } // namespace nav_2d_utils
          
          #endif // NAV_2D_UTILS__CONVERSIONS_HPP_

navigation2/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
          #define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
          
          #include <cmath>
          #include <memory>
          #include <mutex>
          #include <string>
          #include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
          #include "nav_msgs/msg/odometry.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace nav_2d_utils
          {
          
          /**
           * @class OdomSubscriber
           * Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
           */
      55  class OdomSubscriber
          {
          public:
           /**
           * @brief Constructor that subscribes to an Odometry topic
           *
           * @param nh NodeHandle for creating subscriber
           * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
           */
      64   explicit OdomSubscriber(  
      65   nav2_util::LifecycleNode::SharedPtr nh,  
      66   std::string default_topic = "odom" )
           {
           nav2_util::declare_parameter_if_not_declared(  
           nh,   "odom_topic",   rclcpp::ParameterValue(  default_topic ) );
          
           std::string odom_topic;
           nh->get_parameter_or(  "odom_topic",   odom_topic,   default_topic );
           odom_sub_ =
           nh->create_subscription<nav_msgs::msg::Odometry>(  
           odom_topic,  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &OdomSubscriber::odomCallback,   this,   std::placeholders::_1 ) );
           }
          
      80   inline nav_2d_msgs::msg::Twist2D getTwist(   ) {return odom_vel_.velocity;}
      81   inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped(   ) {return odom_vel_;}
          
          protected:
      84   void odomCallback(  const nav_msgs::msg::Odometry::SharedPtr msg )
           {
           // ROS_INFO_ONCE(  "odom received!" );
           std::lock_guard<std::mutex> lock(  odom_mutex_ );
           odom_vel_.header = msg->header;
           odom_vel_.velocity.x = msg->twist.twist.linear.x;
           odom_vel_.velocity.y = msg->twist.twist.linear.y;
           odom_vel_.velocity.theta = msg->twist.twist.angular.z;
           }
          
      94   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
      95   nav_2d_msgs::msg::Twist2DStamped odom_vel_;
      96   std::mutex odom_mutex_;
          };
          
          } // namespace nav_2d_utils
          
          #endif // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_

navigation2/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV_2D_UTILS__PARAMETERS_HPP_
          #define NAV_2D_UTILS__PARAMETERS_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          
          // TODO(  crdelsey ): Remove when code is re-enabled
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wunused-parameter"
          namespace nav_2d_utils
          {
          
          /**
           * @brief Search for a parameter and load it,   or use the default value
           *
           * This templated function shortens a commonly used ROS pattern in which you
           * search for a parameter and get its value if it exists,   otherwise returning a default value.
           *
           * @param nh NodeHandle to start the parameter search from
           * @param param_name Name of the parameter to search for
           * @param default_value Value to return if not found
           * @return Value of parameter if found,   otherwise the default_value
           */
          template<class param_t>
      63  param_t searchAndGetParam(  
           const nav2_util::LifecycleNode::SharedPtr & nh,   const std::string & param_name,  
           const param_t & default_value )
          {
           param_t value;
           nav2_util::declare_parameter_if_not_declared(  
           nh,   param_name,  
           rclcpp::ParameterValue(  default_value ) );
           nh->get_parameter(  param_name,   value );
           return value;
          }
          
          } // namespace nav_2d_utils
          #pragma GCC diagnostic pop
          
          #endif // NAV_2D_UTILS__PARAMETERS_HPP_

navigation2/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/path_ops.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV_2D_UTILS__PATH_OPS_HPP_
          #define NAV_2D_UTILS__PATH_OPS_HPP_
          
          #include "nav_2d_msgs/msg/path2_d.hpp"
          
          namespace nav_2d_utils
          {
          /**
           * @brief Increase plan resolution to match that of the costmap by adding points linearly between points
           *
           * @param global_plan_in input plan
           * @param resolution desired distance between waypoints
           * @return Higher resolution plan
           */
      49  nav_2d_msgs::msg::Path2D adjustPlanResolution(  
           const nav_2d_msgs::msg::Path2D & global_plan_in,  
           double resolution );
          } // namespace nav_2d_utils
          
          #endif // NAV_2D_UTILS__PATH_OPS_HPP_

navigation2/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #ifndef NAV_2D_UTILS__TF_HELP_HPP_
          #define NAV_2D_UTILS__TF_HELP_HPP_
          
          #include <string>
          #include <memory>
          #include "tf2_ros/buffer.h"
          #include "nav_2d_utils/conversions.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          
          namespace nav_2d_utils
          {
          /**
           * @brief Transform a PoseStamped from one frame to another while catching exceptions
           *
           * Also returns immediately if the frames are equal.
           * @param tf Smart pointer to TFListener
           * @param frame Frame to transform the pose into
           * @param in_pose Pose to transform
           * @param out_pose Place to store the resulting transformed pose
           * @return True if successful transform
           */
      58  bool transformPose(  
           const std::shared_ptr<tf2_ros::Buffer> tf,  
           const std::string frame,  
           const geometry_msgs::msg::PoseStamped & in_pose,  
           geometry_msgs::msg::PoseStamped & out_pose,  
           rclcpp::Duration & transform_tolerance
           );
          
          /**
           * @brief Transform a Pose2DStamped from one frame to another while catching exceptions
           *
           * Also returns immediately if the frames are equal.
           * @param tf Smart pointer to TFListener
           * @param frame Frame to transform the pose into
           * @param in_pose Pose to transform
           * @param out_pose Place to store the resulting transformed pose
           * @return True if successful transform
           */
      76  bool transformPose(  
           const std::shared_ptr<tf2_ros::Buffer> tf,  
           const std::string frame,  
           const nav_2d_msgs::msg::Pose2DStamped & in_pose,  
           nav_2d_msgs::msg::Pose2DStamped & out_pose,  
           rclcpp::Duration & transform_tolerance
           );
          
          } // namespace nav_2d_utils
          
          #endif // NAV_2D_UTILS__TF_HELP_HPP_

navigation2/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav_2d_utils/conversions.hpp"
          
          #include <vector>
          #include <string>
          
          #include "geometry_msgs/msg/pose.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #pragma GCC diagnostic push
          #pragma GCC diagnostic ignored "-Wpedantic"
          #include "tf2/utils.h"
          #pragma GCC diagnostic pop
          
          #include "nav2_util/geometry_utils.hpp"
          
          namespace nav_2d_utils
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
      56  geometry_msgs::msg::Twist twist2Dto3D(  const nav_2d_msgs::msg::Twist2D & cmd_vel_2d )
          {
           geometry_msgs::msg::Twist cmd_vel;
           cmd_vel.linear.x = cmd_vel_2d.x;
           cmd_vel.linear.y = cmd_vel_2d.y;
           cmd_vel.angular.z = cmd_vel_2d.theta;
           return cmd_vel;
          }
          
      65  nav_2d_msgs::msg::Twist2D twist3Dto2D(  const geometry_msgs::msg::Twist & cmd_vel )
          {
           nav_2d_msgs::msg::Twist2D cmd_vel_2d;
           cmd_vel_2d.x = cmd_vel.linear.x;
           cmd_vel_2d.y = cmd_vel.linear.y;
           cmd_vel_2d.theta = cmd_vel.angular.z;
           return cmd_vel_2d;
          }
          
          // nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(  const tf2::Stamped<tf2::Pose>& pose )
          // {
          // nav_2d_msgs::msg::Pose2DStamped pose2d;
          // pose2d.header.stamp = pose.stamp_;
          // pose2d.header.frame_id = pose.frame_id_;
          // pose2d.pose.x = pose.getOrigin(   ).getX(   );
          // pose2d.pose.y = pose.getOrigin(   ).getY(   );
          // pose2d.pose.theta = tf::getYaw(  pose.getRotation(   ) );
          // return pose2d;
          // }
          
      85  nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(  const geometry_msgs::msg::PoseStamped & pose )
          {
           nav_2d_msgs::msg::Pose2DStamped pose2d;
           pose2d.header = pose.header;
           pose2d.pose.x = pose.pose.position.x;
           pose2d.pose.y = pose.pose.position.y;
           pose2d.pose.theta = tf2::getYaw(  pose.pose.orientation );
           return pose2d;
          }
          
      95  geometry_msgs::msg::Pose2D poseToPose2D(  const geometry_msgs::msg::Pose & pose )
          {
           geometry_msgs::msg::Pose2D pose2d;
           pose2d.x = pose.position.x;
           pose2d.y = pose.position.y;
           pose2d.theta = tf2::getYaw(  pose.orientation );
           return pose2d;
          }
          
     104  geometry_msgs::msg::Pose pose2DToPose(  const geometry_msgs::msg::Pose2D & pose2d )
          {
           geometry_msgs::msg::Pose pose;
           pose.position.x = pose2d.x;
           pose.position.y = pose2d.y;
           pose.orientation = orientationAroundZAxis(  pose2d.theta );
           return pose;
          }
          
     113  geometry_msgs::msg::PoseStamped pose2DToPoseStamped(  
           const nav_2d_msgs::msg::Pose2DStamped & pose2d )
          {
           geometry_msgs::msg::PoseStamped pose;
           pose.header = pose2d.header;
           pose.pose = pose2DToPose(  pose2d.pose );
           return pose;
          }
          
     122  geometry_msgs::msg::PoseStamped pose2DToPoseStamped(  
           const geometry_msgs::msg::Pose2D & pose2d,  
           const std::string & frame,   const rclcpp::Time & stamp )
          {
           geometry_msgs::msg::PoseStamped pose;
           pose.header.frame_id = frame;
           pose.header.stamp = stamp;
           pose.pose.position.x = pose2d.x;
           pose.pose.position.y = pose2d.y;
           pose.pose.orientation = orientationAroundZAxis(  pose2d.theta );
           return pose;
          }
          
     135  nav_msgs::msg::Path posesToPath(  const std::vector<geometry_msgs::msg::PoseStamped> & poses )
          {
           nav_msgs::msg::Path path;
           if (  poses.empty(   ) ) {
           return path;
           }
           path.poses.resize(  poses.size(   ) );
           path.header.frame_id = poses[0].header.frame_id;
           path.header.stamp = poses[0].header.stamp;
           for (  unsigned int i = 0; i < poses.size(   ); i++ ) {
           path.poses[i] = poses[i];
           }
           return path;
          }
          
     150  nav_2d_msgs::msg::Path2D pathToPath2D(  const nav_msgs::msg::Path & path )
          {
           nav_2d_msgs::msg::Path2D path2d;
           path2d.header = path.header;
           for (  auto & pose : path.poses ) {
           path2d.poses.push_back(  poseToPose2D(  pose.pose ) );
           }
           return path2d;
          }
          
          
     161  nav_msgs::msg::Path poses2DToPath(  
           const std::vector<geometry_msgs::msg::Pose2D> & poses,  
           const std::string & frame,   const rclcpp::Time & stamp )
          {
           nav_msgs::msg::Path path;
           path.poses.resize(  poses.size(   ) );
           path.header.frame_id = frame;
           path.header.stamp = stamp;
           for (  unsigned int i = 0; i < poses.size(   ); i++ ) {
           path.poses[i] = pose2DToPoseStamped(  poses[i],   frame,   stamp );
           }
           return path;
          }
          
     175  nav_msgs::msg::Path pathToPath(  const nav_2d_msgs::msg::Path2D & path2d )
          {
           nav_msgs::msg::Path path;
           path.header = path2d.header;
           path.poses.resize(  path2d.poses.size(   ) );
           for (  unsigned int i = 0; i < path.poses.size(   ); i++ ) {
           path.poses[i].header = path2d.header;
           path.poses[i].pose = pose2DToPose(  path2d.poses[i] );
           }
           return path;
          }
          
          } // namespace nav_2d_utils

navigation2/nav2_dwb_controller/nav_2d_utils/src/path_ops.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav_2d_utils/path_ops.hpp"
          #include <cmath>
          
          using std::sqrt;
          
          namespace nav_2d_utils
          {
      42  nav_2d_msgs::msg::Path2D adjustPlanResolution(  
           const nav_2d_msgs::msg::Path2D & global_plan_in,  
           double resolution )
          {
           nav_2d_msgs::msg::Path2D global_plan_out;
           if (  global_plan_in.poses.size(   ) == 0 ) {
           return global_plan_out;
           }
          
           geometry_msgs::msg::Pose2D last = global_plan_in.poses[0];
           global_plan_out.poses.push_back(  last );
          
           // we can take "holes" in the plan smaller than 2 grid cells (  squared = 4 )
           double min_sq_resolution = resolution * resolution * 4.0;
          
           for (  unsigned int i = 1; i < global_plan_in.poses.size(   ); ++i ) {
           geometry_msgs::msg::Pose2D loop = global_plan_in.poses[i];
           double sq_dist = (  loop.x - last.x ) * (  loop.x - last.x ) + (  loop.y - last.y ) * (  loop.y - last.y );
           if (  sq_dist > min_sq_resolution ) {
           // add points in-between
           double diff = sqrt(  sq_dist ) - sqrt(  min_sq_resolution );
           int steps = static_cast<int>(  diff / resolution ) - 1;
           double steps_double = static_cast<double>(  steps );
          
           double delta_x = (  loop.x - last.x ) / steps_double;
           double delta_y = (  loop.y - last.y ) / steps_double;
           double delta_t = (  loop.theta - last.theta ) / steps_double;
          
           for (  int j = 1; j < steps; ++j ) {
           geometry_msgs::msg::Pose2D pose;
           pose.x = last.x + j * delta_x;
           pose.y = last.y + j * delta_y;
           pose.theta = last.theta + j * delta_t;
           global_plan_out.poses.push_back(  pose );
           }
           }
           global_plan_out.poses.push_back(  global_plan_in.poses[i] );
           last.x = loop.x;
           last.y = loop.y;
           }
           return global_plan_out;
          }
          } // namespace nav_2d_utils

navigation2/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2017,   Locus Robotics
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          #include "nav_2d_utils/tf_help.hpp"
          
          namespace nav_2d_utils
          {
          
      42  bool transformPose(  
           const std::shared_ptr<tf2_ros::Buffer> tf,  
           const std::string frame,  
           const geometry_msgs::msg::PoseStamped & in_pose,  
           geometry_msgs::msg::PoseStamped & out_pose,  
           rclcpp::Duration & transform_tolerance
           )
          {
           if (  in_pose.header.frame_id == frame ) {
           out_pose = in_pose;
           return true;
           }
          
           try {
           tf->transform(  in_pose,   out_pose,   frame );
           return true;
           } catch (  tf2::ExtrapolationException & ex ) {
           auto transform = tf->lookupTransform(  
           frame,  
           in_pose.header.frame_id,  
           tf2::TimePointZero
            );
           if (  
           (  rclcpp::Time(  in_pose.header.stamp ) - rclcpp::Time(  transform.header.stamp ) ) >
           transform_tolerance )
           {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "tf_help" ),  
           "Transform data too old when converting from %s to %s",  
           in_pose.header.frame_id.c_str(   ),  
           frame.c_str(   )
            );
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "tf_help" ),  
           "Data time: %ds %uns,   Transform time: %ds %uns",  
           in_pose.header.stamp.sec,  
           in_pose.header.stamp.nanosec,  
           transform.header.stamp.sec,  
           transform.header.stamp.nanosec
            );
           return false;
           } else {
           tf2::doTransform(  in_pose,   out_pose,   transform );
           return true;
           }
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "tf_help" ),  
           "Exception in transformPose: %s",  
           ex.what(   )
            );
           return false;
           }
           return false;
          }
          
      98  bool transformPose(  
           const std::shared_ptr<tf2_ros::Buffer> tf,  
           const std::string frame,  
           const nav_2d_msgs::msg::Pose2DStamped & in_pose,  
           nav_2d_msgs::msg::Pose2DStamped & out_pose,  
           rclcpp::Duration & transform_tolerance
           )
          {
           geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(  in_pose );
           geometry_msgs::msg::PoseStamped out_3d_pose;
          
           bool ret = transformPose(  tf,   frame,   in_3d_pose,   out_3d_pose,   transform_tolerance );
           if (  ret ) {
           out_pose = poseStampedToPose2D(  out_3d_pose );
           }
           return ret;
          }
          } // namespace nav_2d_utils

navigation2/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <math.h>
          #include <tf2/LinearMath/Quaternion.h>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav_2d_utils/conversions.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp/time.hpp"
          
          using nav_2d_utils::posesToPath;
          using nav_2d_utils::pathToPath;
          
      47  TEST(  nav_2d_utils,   PosesToPathEmpty )
          {
           std::vector<geometry_msgs::msg::PoseStamped> poses;
           nav_msgs::msg::Path path = posesToPath(  poses );
          
           EXPECT_EQ(  path.poses.size(   ),   0ul );
          }
          
      55  TEST(  nav_2d_utils,   PosesToPathNonEmpty )
          {
           std::vector<geometry_msgs::msg::PoseStamped> poses;
           geometry_msgs::msg::PoseStamped pose1;
           rclcpp::Time time1,   time2;
           auto node = rclcpp::Node::make_shared(  "twod_utils_test_node" );
           time1 = node->now(   );
          
           tf2::Quaternion quat1,   quat2;
           quat1.setRPY(  0,   0,   0.123 );
           pose1.pose.position.x = 1.0;
           pose1.pose.position.y = 2.0;
           pose1.pose.orientation.w = quat1.w(   );
           pose1.pose.orientation.x = quat1.x(   );
           pose1.pose.orientation.y = quat1.y(   );
           pose1.pose.orientation.z = quat1.z(   );
           pose1.header.stamp = time1;
           pose1.header.frame_id = "frame1_id";
          
           geometry_msgs::msg::PoseStamped pose2;
           pose2.pose.position.x = 4.0;
           pose2.pose.position.y = 5.0;
           quat2.setRPY(  0,   0,   0.987 );
           pose2.pose.orientation.w = quat2.w(   );
           pose2.pose.orientation.x = quat2.x(   );
           pose2.pose.orientation.y = quat2.y(   );
           pose2.pose.orientation.z = quat2.z(   );
          
           time2 = node->now(   );
           pose2.header.stamp = time2;
           pose2.header.frame_id = "frame2_id";
          
           poses.push_back(  pose1 );
           poses.push_back(  pose2 );
          
           nav_msgs::msg::Path path = posesToPath(  poses );
          
           EXPECT_EQ(  path.poses.size(   ),   2ul );
           EXPECT_EQ(  path.poses[0].pose.position.x,   1.0 );
           EXPECT_EQ(  path.poses[0].pose.position.y,   2.0 );
           EXPECT_EQ(  path.poses[0].header.stamp,   time1 );
           EXPECT_EQ(  path.poses[0].header.frame_id,   "frame1_id" );
           EXPECT_EQ(  path.poses[1].pose.position.x,   4.0 );
           EXPECT_EQ(  path.poses[1].pose.position.y,   5.0 );
           EXPECT_EQ(  path.poses[1].header.frame_id,   "frame2_id" );
          
           EXPECT_EQ(  path.header.stamp,   time1 );
          }
          
     104  TEST(  nav_2d_utils,   PathToPathEmpty )
          {
           nav_2d_msgs::msg::Path2D path2d;
           nav_msgs::msg::Path path = pathToPath(  path2d );
           EXPECT_EQ(  path.poses.size(   ),   0ul );
          }
          
     111  TEST(  nav_2d_utils,   PathToPathNoNEmpty )
          {
           nav_2d_msgs::msg::Path2D path2d;
          
           geometry_msgs::msg::Pose2D pose1;
           pose1.x = 1.0;
           pose1.y = 2.0;
           pose1.theta = M_PI / 2.0;
          
           geometry_msgs::msg::Pose2D pose2;
           pose2.x = 4.0;
           pose2.y = 5.0;
           pose2.theta = M_PI;
          
           path2d.poses.push_back(  pose1 );
           path2d.poses.push_back(  pose2 );
          
           nav_msgs::msg::Path path = pathToPath(  path2d );
           EXPECT_EQ(  path.poses.size(   ),   2ul );
           EXPECT_EQ(  path.poses[0].pose.position.x,   1.0 );
           EXPECT_EQ(  path.poses[0].pose.position.y,   2.0 );
          
           tf2::Quaternion quat;
           quat.setRPY(  0,   0,   M_PI / 2.0 );
           EXPECT_EQ(  path.poses[0].pose.orientation.w,   quat.w(   ) );
           EXPECT_EQ(  path.poses[0].pose.orientation.x,   quat.x(   ) );
           EXPECT_EQ(  path.poses[0].pose.orientation.y,   quat.x(   ) );
           EXPECT_EQ(  path.poses[0].pose.orientation.z,   quat.z(   ) );
          
           EXPECT_EQ(  path.poses[1].pose.position.x,   4.0 );
           EXPECT_EQ(  path.poses[1].pose.position.y,   5.0 );
           quat.setRPY(  0,   0,   M_PI );
           EXPECT_EQ(  path.poses[1].pose.orientation.w,   quat.w(   ) );
           EXPECT_EQ(  path.poses[1].pose.orientation.x,   quat.x(   ) );
           EXPECT_EQ(  path.poses[1].pose.orientation.y,   quat.x(   ) );
           EXPECT_EQ(  path.poses[1].pose.orientation.z,   quat.z(   ) );
          }
          
     149  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <cmath>
          #include "gtest/gtest.h"
          #include "nav_2d_utils/path_ops.hpp"
          
          using std::sqrt;
          using nav_2d_utils::adjustPlanResolution;
          
      42  TEST(  path_ops_test,   AdjustResolutionEmpty )
          {
           nav_2d_msgs::msg::Path2D in;
           nav_2d_msgs::msg::Path2D out = adjustPlanResolution(  in,   2.0 );
           EXPECT_EQ(  out.poses.size(   ),   0ul );
          }
          
      49  TEST(  path_ops_test,   AdjustResolutionSimple )
          {
           nav_2d_msgs::msg::Path2D in;
           const float RESOLUTION = 20.0;
          
           geometry_msgs::msg::Pose2D pose1;
           pose1.x = 0.0;
           pose1.y = 0.0;
           geometry_msgs::msg::Pose2D pose2;
           pose2.x = 100.0;
           pose2.y = 0.0;
          
           in.poses.push_back(  pose1 );
           in.poses.push_back(  pose2 );
          
           nav_2d_msgs::msg::Path2D out = adjustPlanResolution(  in,   RESOLUTION );
           float length = 100;
           ulong number_of_points = ceil(  length / (  2 * RESOLUTION ) );
           EXPECT_EQ(  out.poses.size(   ),   number_of_points );
           float max_length = length / (  number_of_points - 1 );
          
           for (  unsigned int i = 1; i < out.poses.size(   ); i++ ) {
           pose1 = out.poses[i - 1];
           pose2 = out.poses[i];
          
           double sq_dist = (  pose1.x - pose2.x ) * (  pose1.x - pose2.x ) +
           (  pose1.y - pose2.y ) * (  pose1.y - pose2.y );
          
           EXPECT_TRUE(  sqrt(  sq_dist ) <= max_length );
           }
          }

navigation2/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp

       1  /*
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2018,   Wilco Bonestroo
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the copyright holder nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include <memory>
          #include <string>
          
          #include "gtest/gtest.h"
          #include "nav_2d_utils/tf_help.hpp"
          
      41  TEST(  TF_Help,   TransformToSelf ) {
           bool result;
          
           std::shared_ptr<tf2_ros::Buffer> tf;
           std::string frame = "frame_id";
           geometry_msgs::msg::PoseStamped in_pose;
           in_pose.header.frame_id = "frame_id";
           in_pose.pose.position.x = 1.0;
           in_pose.pose.position.y = 2.0;
           in_pose.pose.position.z = 3.0;
           tf2::Quaternion qt;
           qt.setRPY(  0.5,   1.0,   1.5 );
           in_pose.pose.orientation.w = qt.w(   );
           in_pose.pose.orientation.x = qt.x(   );
           in_pose.pose.orientation.y = qt.y(   );
           in_pose.pose.orientation.z = qt.z(   );
          
           geometry_msgs::msg::PoseStamped out_pose;
           rclcpp::Duration transform_tolerance(  0,   500 );
          
           result = nav_2d_utils::transformPose(  tf,   frame,   in_pose,   out_pose,   transform_tolerance );
          
           EXPECT_TRUE(  result );
           EXPECT_EQ(  out_pose.header.frame_id,   "frame_id" );
           EXPECT_EQ(  out_pose.pose.position.x,   1.0 );
           EXPECT_EQ(  out_pose.pose.position.y,   2.0 );
           EXPECT_EQ(  out_pose.pose.position.z,   3.0 );
           EXPECT_EQ(  out_pose.pose.orientation.w,   qt.w(   ) );
           EXPECT_EQ(  out_pose.pose.orientation.x,   qt.x(   ) );
           EXPECT_EQ(  out_pose.pose.orientation.y,   qt.y(   ) );
           EXPECT_EQ(  out_pose.pose.orientation.z,   qt.z(   ) );
          }
          
      74  TEST(  TF_Help,   EmptyBuffer ) {
           auto clock = std::make_shared<rclcpp::Clock>(  RCL_ROS_TIME );
           auto buffer = std::make_shared<tf2_ros::Buffer>(  clock );
          
           std::string frame = "frame_id";
           geometry_msgs::msg::PoseStamped in_pose;
           in_pose.header.frame_id = "other_frame_id";
           in_pose.pose.position.x = 1.0;
           in_pose.pose.position.y = 2.0;
           in_pose.pose.position.z = 3.0;
           tf2::Quaternion qt;
           qt.setRPY(  0.5,   1.0,   1.5 );
           in_pose.pose.orientation.w = qt.w(   );
           in_pose.pose.orientation.x = qt.x(   );
           in_pose.pose.orientation.y = qt.y(   );
           in_pose.pose.orientation.z = qt.z(   );
          
           geometry_msgs::msg::PoseStamped out_pose;
           rclcpp::Duration transform_tolerance(  0,   500 );
          
           bool result;
           result = nav_2d_utils::transformPose(  buffer,   frame,   in_pose,   out_pose,   transform_tolerance );
          
           EXPECT_FALSE(  result );
          }

navigation2/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

          // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2022 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
          #define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
          
          #include <map>
          #include <memory>
          #include <string>
          #include <thread>
          #include <unordered_map>
          #include <vector>
          
          #include "nav2_util/lifecycle_service_client.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "std_srvs/srv/empty.hpp"
          #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
          #include "std_srvs/srv/trigger.hpp"
          #include "bondcpp/bond.hpp"
          #include "diagnostic_updater/diagnostic_updater.hpp"
          
          
          namespace nav2_lifecycle_manager
          {
          using namespace std::chrono_literals; // NOLINT
          
          using nav2_msgs::srv::ManageLifecycleNodes;
          /**
           * @class nav2_lifecycle_manager::LifecycleManager
           * @brief Implements service interface to transition the lifecycle nodes of
           * Nav2 stack. It receives transition request and then uses lifecycle
           * interface to change lifecycle node's state.
           */
      47  class LifecycleManager : public rclcpp::Node
          {
          public:
           /**
           * @brief A constructor for nav2_lifecycle_manager::LifecycleManager
           * @param options Additional options to control creation of the node.
           */
      54   explicit LifecycleManager(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief A destructor for nav2_lifecycle_manager::LifecycleManager
           */
      58   ~LifecycleManager(   );
          
          protected:
           // Callback group used by services and timers
      62   rclcpp::CallbackGroup::SharedPtr callback_group_;
      63   std::unique_ptr<nav2_util::NodeThread> service_thread_;
          
           // The services provided by this node
      66   rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
      67   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
           /**
           * @brief Lifecycle node manager callback function
           * @param request_header Header of the service request
           * @param request Service request
           * @param reponse Service response
           */
      74   void managerCallback(  
      75   const std::shared_ptr<rmw_request_id_t> request_header,  
      76   const std::shared_ptr<ManageLifecycleNodes::Request> request,  
      77   std::shared_ptr<ManageLifecycleNodes::Response> response );
           /**
           * @brief Trigger callback function checks if the managed nodes are in active
           * state.
           * @param request_header Header of the request
           * @param request Service request
           * @param reponse Service response
           */
      85   void isActiveCallback(  
      86   const std::shared_ptr<rmw_request_id_t> request_header,  
      87   const std::shared_ptr<std_srvs::srv::Trigger::Request> request,  
      88   std::shared_ptr<std_srvs::srv::Trigger::Response> response );
          
           // Support functions for the service calls
           /**
           * @brief Start up managed nodes.
           * @return true or false
           */
      95   bool startup(   );
           /**
           * @brief Deactivate,   clean up and shut down all the managed nodes.
           * @return true or false
           */
     100   bool shutdown(   );
           /**
           * @brief Reset all the managed nodes.
           * @return true or false
           */
     105   bool reset(  bool hard_reset = false );
           /**
           * @brief Pause all the managed nodes.
           * @return true or false
           */
     110   bool pause(   );
           /**
           * @brief Resume all the managed nodes.
           * @return true or false
           */
     115   bool resume(   );
          
           // Support function for creating service clients
           /**
           * @brief Support function for creating service clients
           */
     121   void createLifecycleServiceClients(   );
          
           // Support functions for shutdown
           /**
           * @brief Support function for shutdown
           */
     127   void shutdownAllNodes(   );
           /**
           * @brief Destroy all the lifecycle service clients.
           */
     131   void destroyLifecycleServiceClients(   );
          
           // Support function for creating bond timer
           /**
           * @brief Support function for creating bond timer
           */
     137   void createBondTimer(   );
          
           // Support function for creating bond connection
           /**
           * @brief Support function for creating bond connections
           */
     143   bool createBondConnection(  const std::string & node_name );
          
           // Support function for killing bond connections
           /**
           * @brief Support function for killing bond connections
           */
     149   void destroyBondTimer(   );
          
           // Support function for checking on bond connections
           /**
           * @ brief Support function for checking on bond connections
           * will take down system if there's something non-responsive
           */
     156   void checkBondConnections(   );
          
           // Support function for checking if bond connections come back after respawn
           /**
           * @ brief Support function for checking on bond connections
           * will bring back the system if something goes from non-responsive to responsive
           */
     163   void checkBondRespawnConnection(   );
          
           /**
           * @brief For a node,   transition to the new target state
           */
     168   bool changeStateForNode(  
     169   const std::string & node_name,  
     170   std::uint8_t transition );
          
           /**
           * @brief For each node in the map,   transition to the new target state
           */
     175   bool changeStateForAllNodes(  std::uint8_t transition,   bool hard_change = false );
          
           // Convenience function to highlight the output on the console
           /**
           * @brief Helper function to highlight the output on the console
           */
     181   void message(  const std::string & msg );
          
           // Diagnostics functions
           /**
           * @brief function to check if the Nav2 system is active
           */
     187   void CreateActiveDiagnostic(  diagnostic_updater::DiagnosticStatusWrapper & stat );
          
           // Timer thread to look at bond connections
     190   rclcpp::TimerBase::SharedPtr init_timer_;
     191   rclcpp::TimerBase::SharedPtr bond_timer_;
     192   rclcpp::TimerBase::SharedPtr bond_respawn_timer_;
     193   std::chrono::milliseconds bond_timeout_;
          
           // A map of all nodes to check bond connection
           std::map<std::string,   std::shared_ptr<bond::Bond>> bond_map_;
          
           // A map of all nodes to be controlled
           std::map<std::string,   std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
          
           std::map<std::uint8_t,   std::string> transition_label_map_;
          
           // A map of the expected transitions to primary states
           std::unordered_map<std::uint8_t,   std::uint8_t> transition_state_map_;
          
           // The names of the nodes to be managed,   in the order of desired bring-up
           std::vector<std::string> node_names_;
          
           // Whether to automatically start up the system
           bool autostart_;
           bool attempt_respawn_reconnection_;
          
           bool system_active_{false};
           diagnostic_updater::Updater diagnostics_updater_;
          
           rclcpp::Time bond_respawn_start_time_{0};
     217   rclcpp::Duration bond_respawn_max_duration_{10s};
          };
          
          } // namespace nav2_lifecycle_manager
          
          #endif // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_

navigation2/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
          #define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
          
          #include <memory>
          #include <string>
          
          #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "std_srvs/srv/empty.hpp"
          #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
          #include "std_srvs/srv/trigger.hpp"
          #include "nav2_util/service_client.hpp"
          
          namespace nav2_lifecycle_manager
          {
          /**
           * @enum nav2_lifecycle_manager::SystemStatus
           * @brief Enum class representing the status of the system.
           */
      37  enum class SystemStatus {ACTIVE,   INACTIVE,   TIMEOUT};
          /**
           * @class nav2_lifecycle_manager::LifeCycleMangerClient
           * @brief The LifecycleManagerClient sends requests to the LifecycleManager to
           * control the lifecycle state of the navigation modules.
           */
      43  class LifecycleManagerClient
          {
          public:
           /**
           * @brief A constructor for LifeCycleMangerClient
           * @param name Managed node name
           * @param parent_node Node that execute the service calls
           */
      51   explicit LifecycleManagerClient(  
      52   const std::string & name,  
      53   std::shared_ptr<rclcpp::Node> parent_node );
          
           // Client-side interface to the Nav2 lifecycle manager
           /**
           * @brief Make start up service call
           * @return true or false
           */
      60   bool startup(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
           /**
           * @brief Make shutdown service call
           * @return true or false
           */
      65   bool shutdown(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
           /**
           * @brief Make pause service call
           * @return true or false
           */
      70   bool pause(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
           /**
           * @brief Make resume service call
           * @return true or false
           */
      75   bool resume(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
           /**
           * @brief Make reset service call
           * @return true or false
           */
      80   bool reset(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
           /**
           * @brief Check if lifecycle node manager server is active
           * @return ACTIVE or INACTIVE or TIMEOUT
           */
      85   SystemStatus is_active(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
          
           // A couple convenience methods to facilitate scripting tests
           /**
           * @brief Set initial pose with covariance
           * @param x X position
           * @param y Y position
           * @param theta orientation
           */
      94   void set_initial_pose(  double x,   double y,   double theta );
           /**
           * @brief Send goal pose to NavigationToPose action server
           * @param x X position
           * @param y Y position
           * @param theta orientation
           * @return true or false
           */
     102   bool navigate_to_pose(  double x,   double y,   double theta );
          
          protected:
           using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes;
          
           /**
           * @brief A generic method used to call startup,   shutdown,   etc.
           * @param command
           */
           bool callService(  
           uint8_t command,  
           const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) );
          
           // The node to use for the service call
           rclcpp::Node::SharedPtr node_;
          
           std::shared_ptr<nav2_util::ServiceClient<ManageLifecycleNodes>> manager_client_;
           std::shared_ptr<nav2_util::ServiceClient<std_srvs::srv::Trigger>> is_active_client_;
           std::string manage_service_name_;
           std::string active_service_name_;
          };
          
          } // namespace nav2_lifecycle_manager
          
          #endif // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_

navigation2/nav2_lifecycle_manager/src/lifecycle_manager.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2022 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_lifecycle_manager/lifecycle_manager.hpp"
          
          #include <chrono>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::placeholders;
          
          using lifecycle_msgs::msg::Transition;
          using lifecycle_msgs::msg::State;
          using nav2_util::LifecycleServiceClient;
          
          namespace nav2_lifecycle_manager
          {
          
      35  LifecycleManager::LifecycleManager(  const rclcpp::NodeOptions & options )
          : Node(  "lifecycle_manager",   options ),   diagnostics_updater_(  this )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           // The list of names is parameterized,   allowing this module to be used with a different set
           // of nodes
           declare_parameter(  "node_names",   rclcpp::PARAMETER_STRING_ARRAY );
           declare_parameter(  "autostart",   rclcpp::ParameterValue(  false ) );
           declare_parameter(  "bond_timeout",   4.0 );
           declare_parameter(  "bond_respawn_max_duration",   10.0 );
           declare_parameter(  "attempt_respawn_reconnection",   true );
          
           node_names_ = get_parameter(  "node_names" ).as_string_array(   );
           get_parameter(  "autostart",   autostart_ );
           double bond_timeout_s;
           get_parameter(  "bond_timeout",   bond_timeout_s );
           bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(  
           std::chrono::duration<double>(  bond_timeout_s ) );
          
           double respawn_timeout_s;
           get_parameter(  "bond_respawn_max_duration",   respawn_timeout_s );
           bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(  respawn_timeout_s );
          
           get_parameter(  "attempt_respawn_reconnection",   attempt_respawn_reconnection_ );
          
           callback_group_ = create_callback_group(  rclcpp::CallbackGroupType::MutuallyExclusive,   false );
           manager_srv_ = create_service<ManageLifecycleNodes>(  
           get_name(   ) + std::string(  "/manage_nodes" ),  
           std::bind(  &LifecycleManager::managerCallback,   this,   _1,   _2,   _3 ),  
           rmw_qos_profile_services_default,  
           callback_group_ );
          
           is_active_srv_ = create_service<std_srvs::srv::Trigger>(  
           get_name(   ) + std::string(  "/is_active" ),  
           std::bind(  &LifecycleManager::isActiveCallback,   this,   _1,   _2,   _3 ),  
           rmw_qos_profile_services_default,  
           callback_group_ );
          
           transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
           transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
           transition_state_map_[Transition::TRANSITION_ACTIVATE] = State::PRIMARY_STATE_ACTIVE;
           transition_state_map_[Transition::TRANSITION_DEACTIVATE] = State::PRIMARY_STATE_INACTIVE;
           transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
           State::PRIMARY_STATE_FINALIZED;
          
           transition_label_map_[Transition::TRANSITION_CONFIGURE] = std::string(  "Configuring " );
           transition_label_map_[Transition::TRANSITION_CLEANUP] = std::string(  "Cleaning up " );
           transition_label_map_[Transition::TRANSITION_ACTIVATE] = std::string(  "Activating " );
           transition_label_map_[Transition::TRANSITION_DEACTIVATE] = std::string(  "Deactivating " );
           transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
           std::string(  "Shutting down " );
          
           init_timer_ = this->create_wall_timer(  
           0s,  
           [this](   ) -> void {
           init_timer_->cancel(   );
           createLifecycleServiceClients(   );
           if (  autostart_ ) {
           init_timer_ = this->create_wall_timer(  
           0s,  
           [this](   ) -> void {
           init_timer_->cancel(   );
           startup(   );
           },  
           callback_group_ );
           }
           auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           executor->add_callback_group(  callback_group_,   get_node_base_interface(   ) );
           service_thread_ = std::make_unique<nav2_util::NodeThread>(  executor );
           } );
           diagnostics_updater_.setHardwareID(  "Nav2" );
           diagnostics_updater_.add(  "Nav2 Health",   this,   &LifecycleManager::CreateActiveDiagnostic );
          }
          
     110  LifecycleManager::~LifecycleManager(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Destroying %s",   get_name(   ) );
           service_thread_.reset(   );
          }
          
          void
     117  LifecycleManager::managerCallback(  
     118   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     119   const std::shared_ptr<ManageLifecycleNodes::Request> request,  
     120   std::shared_ptr<ManageLifecycleNodes::Response> response )
          {
           switch (  request->command ) {
           case ManageLifecycleNodes::Request::STARTUP:
           response->success = startup(   );
           break;
           case ManageLifecycleNodes::Request::RESET:
           response->success = reset(   );
           break;
           case ManageLifecycleNodes::Request::SHUTDOWN:
           response->success = shutdown(   );
           break;
           case ManageLifecycleNodes::Request::PAUSE:
           response->success = pause(   );
           break;
           case ManageLifecycleNodes::Request::RESUME:
           response->success = resume(   );
           break;
           }
          }
          
          void
     142  LifecycleManager::isActiveCallback(  
     143   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     144   const std::shared_ptr<std_srvs::srv::Trigger::Request>/*request*/,  
     145   std::shared_ptr<std_srvs::srv::Trigger::Response> response )
          {
           response->success = system_active_;
          }
          
          void
     151  LifecycleManager::CreateActiveDiagnostic(  diagnostic_updater::DiagnosticStatusWrapper & stat )
          {
           if (  system_active_ ) {
           stat.summary(  diagnostic_msgs::msg::DiagnosticStatus::OK,   "Nav2 is active" );
           } else {
           stat.summary(  diagnostic_msgs::msg::DiagnosticStatus::ERROR,   "Nav2 is inactive" );
           }
          }
          
          void
     161  LifecycleManager::createLifecycleServiceClients(   )
          {
           message(  "Creating and initializing lifecycle service clients" );
           for (  auto & node_name : node_names_ ) {
           node_map_[node_name] =
           std::make_shared<LifecycleServiceClient>(  node_name,   shared_from_this(   ) );
           }
          }
          
          void
     171  LifecycleManager::destroyLifecycleServiceClients(   )
          {
           message(  "Destroying lifecycle service clients" );
           for (  auto & kv : node_map_ ) {
           kv.second.reset(   );
           }
          }
          
          bool
     180  LifecycleManager::createBondConnection(  const std::string & node_name )
          {
           const double timeout_ns =
           std::chrono::duration_cast<std::chrono::nanoseconds>(  bond_timeout_ ).count(   );
           const double timeout_s = timeout_ns / 1e9;
          
           if (  bond_map_.find(  node_name ) == bond_map_.end(   ) && bond_timeout_.count(   ) > 0.0 ) {
           bond_map_[node_name] =
           std::make_shared<bond::Bond>(  "bond",   node_name,   shared_from_this(   ) );
           bond_map_[node_name]->setHeartbeatTimeout(  timeout_s );
           bond_map_[node_name]->setHeartbeatPeriod(  0.10 );
           bond_map_[node_name]->start(   );
           if (  
           !bond_map_[node_name]->waitUntilFormed(  
           rclcpp::Duration(  rclcpp::Duration::from_nanoseconds(  timeout_ns / 2 ) ) ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Server %s was unable to be reached after %0.2fs by bond. "
           "This server may be misconfigured.",  
           node_name.c_str(   ),   timeout_s );
           return false;
           }
           RCLCPP_INFO(  get_logger(   ),   "Server %s connected with bond.",   node_name.c_str(   ) );
           }
          
           return true;
          }
          
          bool
     210  LifecycleManager::changeStateForNode(  const std::string & node_name,   std::uint8_t transition )
          {
           message(  transition_label_map_[transition] + node_name );
          
           if (  !node_map_[node_name]->change_state(  transition ) ||
           !(  node_map_[node_name]->get_state(   ) == transition_state_map_[transition] ) )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to change state for node: %s",   node_name.c_str(   ) );
           return false;
           }
          
           if (  transition == Transition::TRANSITION_ACTIVATE ) {
           return createBondConnection(  node_name );
           } else if (  transition == Transition::TRANSITION_DEACTIVATE ) {
           bond_map_.erase(  node_name );
           }
          
           return true;
          }
          
          bool
     231  LifecycleManager::changeStateForAllNodes(  std::uint8_t transition,   bool hard_change )
          {
           // Hard change will continue even if a node fails
           if (  transition == Transition::TRANSITION_CONFIGURE ||
           transition == Transition::TRANSITION_ACTIVATE )
           {
           for (  auto & node_name : node_names_ ) {
           try {
           if (  !changeStateForNode(  node_name,   transition ) && !hard_change ) {
           return false;
           }
           } catch (  const std::runtime_error & e ) {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Failed to change state for node: %s. Exception: %s.",   node_name.c_str(   ),   e.what(   ) );
           return false;
           }
           }
           } else {
           std::vector<std::string>::reverse_iterator rit;
           for (  rit = node_names_.rbegin(   ); rit != node_names_.rend(   ); ++rit ) {
           try {
           if (  !changeStateForNode(  *rit,   transition ) && !hard_change ) {
           return false;
           }
           } catch (  const std::runtime_error & e ) {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Failed to change state for node: %s. Exception: %s.",   (  *rit ).c_str(   ),   e.what(   ) );
           return false;
           }
           }
           }
           return true;
          }
          
          void
     268  LifecycleManager::shutdownAllNodes(   )
          {
           message(  "Deactivate,   cleanup,   and shutdown nodes" );
           changeStateForAllNodes(  Transition::TRANSITION_DEACTIVATE );
           changeStateForAllNodes(  Transition::TRANSITION_CLEANUP );
           changeStateForAllNodes(  Transition::TRANSITION_UNCONFIGURED_SHUTDOWN );
          }
          
          bool
     277  LifecycleManager::startup(   )
          {
           message(  "Starting managed nodes bringup..." );
           if (  !changeStateForAllNodes(  Transition::TRANSITION_CONFIGURE ) ||
           !changeStateForAllNodes(  Transition::TRANSITION_ACTIVATE ) )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to bring up all requested nodes. Aborting bringup." );
           return false;
           }
           message(  "Managed nodes are active" );
           system_active_ = true;
           createBondTimer(   );
           return true;
          }
          
          bool
     293  LifecycleManager::shutdown(   )
          {
           system_active_ = false;
           destroyBondTimer(   );
          
           message(  "Shutting down managed nodes..." );
           shutdownAllNodes(   );
           destroyLifecycleServiceClients(   );
           message(  "Managed nodes have been shut down" );
           return true;
          }
          
          bool
     306  LifecycleManager::reset(  bool hard_reset )
          {
           system_active_ = false;
           destroyBondTimer(   );
          
           message(  "Resetting managed nodes..." );
           // Should transition in reverse order
           if (  !changeStateForAllNodes(  Transition::TRANSITION_DEACTIVATE,   hard_reset ) ||
           !changeStateForAllNodes(  Transition::TRANSITION_CLEANUP,   hard_reset ) )
           {
           if (  !hard_reset ) {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to reset nodes: aborting reset" );
           return false;
           }
           }
          
           message(  "Managed nodes have been reset" );
           return true;
          }
          
          bool
     327  LifecycleManager::pause(   )
          {
           system_active_ = false;
           destroyBondTimer(   );
          
           message(  "Pausing managed nodes..." );
           if (  !changeStateForAllNodes(  Transition::TRANSITION_DEACTIVATE ) ) {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to pause nodes: aborting pause" );
           return false;
           }
          
           message(  "Managed nodes have been paused" );
           return true;
          }
          
          bool
     343  LifecycleManager::resume(   )
          {
           message(  "Resuming managed nodes..." );
           if (  !changeStateForAllNodes(  Transition::TRANSITION_ACTIVATE ) ) {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to resume nodes: aborting resume" );
           return false;
           }
          
           message(  "Managed nodes are active" );
           system_active_ = true;
           createBondTimer(   );
           return true;
          }
          
          void
     358  LifecycleManager::createBondTimer(   )
          {
           if (  bond_timeout_.count(   ) <= 0 ) {
           return;
           }
          
           message(  "Creating bond timer..." );
           bond_timer_ = this->create_wall_timer(  
           200ms,  
           std::bind(  &LifecycleManager::checkBondConnections,   this ),  
           callback_group_ );
          }
          
          void
     372  LifecycleManager::destroyBondTimer(   )
          {
           if (  bond_timer_ ) {
           message(  "Terminating bond timer..." );
           bond_timer_->cancel(   );
           bond_timer_.reset(   );
           }
          }
          
          void
     382  LifecycleManager::checkBondConnections(   )
          {
           if (  !system_active_ || !rclcpp::ok(   ) || bond_map_.empty(   ) ) {
           return;
           }
          
           for (  auto & node_name : node_names_ ) {
           if (  !rclcpp::ok(   ) ) {
           return;
           }
          
           if (  bond_map_[node_name]->isBroken(   ) ) {
           message(  
           std::string(  
           "Have not received a heartbeat from " + node_name + "." ) );
          
           // if one is down,   bring them all down
           RCLCPP_ERROR(  
           get_logger(   ),  
           "CRITICAL FAILURE: SERVER %s IS DOWN after not receiving a heartbeat for %i ms."
           " Shutting down related nodes.",  
           node_name.c_str(   ),   static_cast<int>(  bond_timeout_.count(   ) ) );
           reset(  true ); // hard reset to transition all still active down
           // if a server crashed,   it won't get cleared due to failed transition,   clear manually
           bond_map_.clear(   );
          
           // Initialize the bond respawn timer to check if server comes back online
           // after a failure,   within a maximum timeout period.
           if (  attempt_respawn_reconnection_ ) {
           bond_respawn_timer_ = this->create_wall_timer(  
           1s,  
           std::bind(  &LifecycleManager::checkBondRespawnConnection,   this ),  
           callback_group_ );
           }
           return;
           }
           }
          }
          
          void
     422  LifecycleManager::checkBondRespawnConnection(   )
          {
           // First attempt in respawn,   start maximum duration to respawn
           if (  bond_respawn_start_time_.nanoseconds(   ) == 0 ) {
           bond_respawn_start_time_ = now(   );
           }
          
           // Note: system_active_ is inverted since this should be in a failure
           // condition. If another outside user actives the system again,   this should not process.
           if (  system_active_ || !rclcpp::ok(   ) || node_names_.empty(   ) ) {
           bond_respawn_start_time_ = rclcpp::Time(  0 );
           bond_respawn_timer_.reset(   );
           return;
           }
          
           // Check number of live connections after a bond failure
           int live_servers = 0;
           const int max_live_servers = node_names_.size(   );
           for (  auto & node_name : node_names_ ) {
           if (  !rclcpp::ok(   ) ) {
           return;
           }
          
           try {
           node_map_[node_name]->get_state(   ); // Only won't throw if the server exists
           live_servers++;
           } catch (  ... ) {
           break;
           }
           }
          
           // If all are alive,   kill timer and retransition system to active
           // Else,   check if maximum timeout has occurred
           if (  live_servers == max_live_servers ) {
           message(  "Successfully re-established connections from server respawns,   starting back up." );
           bond_respawn_start_time_ = rclcpp::Time(  0 );
           bond_respawn_timer_.reset(   );
           startup(   );
           } else if (  now(   ) - bond_respawn_start_time_ >= bond_respawn_max_duration_ ) {
           message(  "Failed to re-establish connection from a server crash after maximum timeout." );
           bond_respawn_start_time_ = rclcpp::Time(  0 );
           bond_respawn_timer_.reset(   );
           }
          }
          
          #define ANSI_COLOR_RESET "\x1b[0m"
          #define ANSI_COLOR_BLUE "\x1b[34m"
          
          void
     471  LifecycleManager::message(  const std::string & msg )
          {
           RCLCPP_INFO(  get_logger(   ),   ANSI_COLOR_BLUE "\33[1m%s\33[0m" ANSI_COLOR_RESET,   msg.c_str(   ) );
          }
          
          } // namespace nav2_lifecycle_manager
          
          #include "rclcpp_components/register_node_macro.hpp"
     479  RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_lifecycle_manager::LifecycleManager )

navigation2/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
          
          #include <cmath>
          #include <memory>
          #include <string>
          #include <utility>
          
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          namespace nav2_lifecycle_manager
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
      29  LifecycleManagerClient::LifecycleManagerClient(  
      30   const std::string & name,  
      31   std::shared_ptr<rclcpp::Node> parent_node )
          {
           manage_service_name_ = name + std::string(  "/manage_nodes" );
           active_service_name_ = name + std::string(  "/is_active" );
          
           // Use parent node for service call and logging
           node_ = parent_node;
          
           // Create the service clients
           manager_client_ = std::make_shared<nav2_util::ServiceClient<ManageLifecycleNodes>>(  
           manage_service_name_,   node_ );
           is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>(  
           active_service_name_,   node_ );
          }
          
          bool
      47  LifecycleManagerClient::startup(  const std::chrono::nanoseconds timeout )
          {
           return callService(  ManageLifecycleNodes::Request::STARTUP,   timeout );
          }
          
          bool
      53  LifecycleManagerClient::shutdown(  const std::chrono::nanoseconds timeout )
          {
           return callService(  ManageLifecycleNodes::Request::SHUTDOWN,   timeout );
          }
          
          bool
      59  LifecycleManagerClient::pause(  const std::chrono::nanoseconds timeout )
          {
           return callService(  ManageLifecycleNodes::Request::PAUSE,   timeout );
          }
          
          bool
      65  LifecycleManagerClient::resume(  const std::chrono::nanoseconds timeout )
          {
           return callService(  ManageLifecycleNodes::Request::RESUME,   timeout );
          }
          
          bool
      71  LifecycleManagerClient::reset(  const std::chrono::nanoseconds timeout )
          {
           return callService(  ManageLifecycleNodes::Request::RESET,   timeout );
          }
          
          SystemStatus
      77  LifecycleManagerClient::is_active(  const std::chrono::nanoseconds timeout )
          {
           auto request = std::make_shared<std_srvs::srv::Trigger::Request>(   );
           auto response = std::make_shared<std_srvs::srv::Trigger::Response>(   );
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Waiting for the %s service...",  
           active_service_name_.c_str(   ) );
          
           if (  !is_active_client_->wait_for_service(  std::chrono::seconds(  1 ) ) ) {
           return SystemStatus::TIMEOUT;
           }
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Sending %s request",  
           active_service_name_.c_str(   ) );
          
           try {
           response = is_active_client_->invoke(  request,   timeout );
           } catch (  std::runtime_error & ) {
           return SystemStatus::TIMEOUT;
           }
          
           if (  response->success ) {
           return SystemStatus::ACTIVE;
           } else {
           return SystemStatus::INACTIVE;
           }
          }
          
          bool
     108  LifecycleManagerClient::callService(  uint8_t command,   const std::chrono::nanoseconds timeout )
          {
           auto request = std::make_shared<ManageLifecycleNodes::Request>(   );
           request->command = command;
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Waiting for the %s service...",  
           manage_service_name_.c_str(   ) );
          
           while (  !manager_client_->wait_for_service(  timeout ) ) {
           if (  !rclcpp::ok(   ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Client interrupted while waiting for service to appear" );
           return false;
           }
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Waiting for service to appear..." );
           }
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "Sending %s request",  
           manage_service_name_.c_str(   ) );
           try {
           auto future_result = manager_client_->invoke(  request,   timeout );
           return future_result->success;
           } catch (  std::runtime_error & ) {
           return false;
           }
          }
          
          } // namespace nav2_lifecycle_manager

navigation2/nav2_lifecycle_manager/src/main.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_lifecycle_manager/lifecycle_manager.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_lifecycle_manager::LifecycleManager>(   );
           rclcpp::spin(  node );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_lifecycle_manager/test/test_bond.cpp

          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <chrono>
          #include <string>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "nav2_lifecycle_manager/lifecycle_manager.hpp"
          #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          // minimal lifecycle node implementing bond as in rest of navigation servers
      28  class TestLifecycleNode : public nav2_util::LifecycleNode
          {
          public:
      31   TestLifecycleNode(  bool bond,   std::string name )
           : nav2_util::LifecycleNode(  name )
           {
           state = "";
           enable_bond = bond;
           }
          
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Configured!" );
           state = "configured";
           return CallbackReturn::SUCCESS;
           }
          
      45   CallbackReturn on_activate(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Activated!" );
           state = "activated";
           if (  enable_bond ) {
           createBond(   );
           }
           return CallbackReturn::SUCCESS;
           }
          
      55   CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Deactivated!" );
           state = "deactivated";
           if (  enable_bond ) {
           destroyBond(   );
           }
           return CallbackReturn::SUCCESS;
           }
          
      65   CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Cleanup!" );
           state = "cleaned up";
           return CallbackReturn::SUCCESS;
           }
          
      72   CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Shutdown!" );
           state = "shut down";
           return CallbackReturn::SUCCESS;
           }
          
      79   CallbackReturn on_error(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is encountered an error!" );
           state = "errored";
           return CallbackReturn::SUCCESS;
           }
          
      86   bool bondAllocated(   )
           {
           return bond_ ? true : false;
           }
          
      91   void breakBond(   )
           {
           bond_->breakBond(   );
           }
          
      96   std::string getState(   )
           {
           return state;
           }
          
     101   bool isBondEnabled(   )
           {
           return enable_bond;
           }
          
     106   bool isBondConnected(   )
           {
           return bondAllocated(   ) ? !bond_->isBroken(   ) : false;
           }
          
           std::string state;
           bool enable_bond;
          };
          
          class TestFixture
          {
          public:
           TestFixture(  bool bond,   std::string node_name )
           {
           lf_node_ = std::make_shared<TestLifecycleNode>(  bond,   node_name );
           lf_thread_ = std::make_unique<nav2_util::NodeThread>(  lf_node_->get_node_base_interface(   ) );
           }
          
           std::shared_ptr<TestLifecycleNode> lf_node_;
           std::unique_ptr<nav2_util::NodeThread> lf_thread_;
          };
          
          TEST(  LifecycleBondTest,   POSITIVE )
          {
           // let the lifecycle server come up
           rclcpp::Rate(  1 ).sleep(   );
          
           auto node = std::make_shared<rclcpp::Node>(  "lifecycle_manager_test_service_client" );
           nav2_lifecycle_manager::LifecycleManagerClient client(  "lifecycle_manager_test",   node );
          
           // create node,   should be up now
           auto fixture = TestFixture(  true,   "bond_tester" );
           auto bond_tester = fixture.lf_node_;
          
           EXPECT_TRUE(  client.startup(   ) );
          
           // check if bond is connected after being activated
           rclcpp::Rate(  5 ).sleep(   );
           EXPECT_TRUE(  bond_tester->isBondConnected(   ) );
           EXPECT_EQ(  bond_tester->getState(   ),   "activated" );
          
           bond_tester->breakBond(   );
          
           // bond should be disconnected now and lifecycle manager should know and react to reset
           rclcpp::Rate(  5 ).sleep(   );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::INACTIVE,  
           client.is_active(  std::chrono::nanoseconds(  1000000000 ) ) );
           EXPECT_FALSE(  bond_tester->isBondConnected(   ) );
           EXPECT_EQ(  bond_tester->getState(   ),   "cleaned up" );
          
           // check that bringing up again is OK
           EXPECT_TRUE(  client.startup(   ) );
           EXPECT_EQ(  bond_tester->getState(   ),   "activated" );
           EXPECT_TRUE(  bond_tester->isBondConnected(   ) );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::ACTIVE,  
           client.is_active(  std::chrono::nanoseconds(  1000000000 ) ) );
          
           // clean state for next test.
           EXPECT_TRUE(  client.reset(   ) );
           EXPECT_FALSE(  bond_tester->isBondConnected(   ) );
           EXPECT_EQ(  bond_tester->getState(   ),   "cleaned up" );
          }
          
          TEST(  LifecycleBondTest,   NEGATIVE )
          {
           auto node = std::make_shared<rclcpp::Node>(  "lifecycle_manager_test_service_client" );
           nav2_lifecycle_manager::LifecycleManagerClient client(  "lifecycle_manager_test",   node );
          
           // create node,   now without bond setup to connect to. Should fail because no bond
           auto fixture = TestFixture(  false,   "bond_tester" );
           auto bond_tester = fixture.lf_node_;
           EXPECT_FALSE(  client.startup(   ) );
           EXPECT_FALSE(  bond_tester->isBondEnabled(   ) );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::INACTIVE,  
           client.is_active(  std::chrono::nanoseconds(  1000000000 ) ) );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp

          // Copyright (  c ) 2020 Shivang Patel
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <chrono>
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      25  class LifecycleNodeTest : public rclcpp_lifecycle::LifecycleNode
          {
          public:
      28   LifecycleNodeTest(   )
           : rclcpp_lifecycle::LifecycleNode(  "lifecycle_node_test" ) {}
          
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Configured!" );
           return CallbackReturn::SUCCESS;
           }
          
      37   CallbackReturn on_activate(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Activated!" );
           return CallbackReturn::SUCCESS;
           }
          
      43   CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Deactivated!" );
           return CallbackReturn::SUCCESS;
           }
          
      49   CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Cleanup!" );
           return CallbackReturn::SUCCESS;
           }
          
      55   CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is Shutdown!" );
           return CallbackReturn::SUCCESS;
           }
          
      61   CallbackReturn on_error(  const rclcpp_lifecycle::State & /*state*/ ) override
           {
           RCLCPP_INFO(  get_logger(   ),   "Lifecycle Test node is encountered an error!" );
           return CallbackReturn::SUCCESS;
           }
          };
          
          class LifecycleClientTestFixture
          {
          public:
           LifecycleClientTestFixture(   )
           {
           lf_node_ = std::make_shared<LifecycleNodeTest>(   );
           lf_thread_ = std::make_unique<nav2_util::NodeThread>(  lf_node_->get_node_base_interface(   ) );
           }
          
          private:
           std::shared_ptr<LifecycleNodeTest> lf_node_;
           std::unique_ptr<nav2_util::NodeThread> lf_thread_;
          };
          
          TEST(  LifecycleClientTest,   BasicTest )
          {
           LifecycleClientTestFixture fix;
           auto node = std::make_shared<rclcpp::Node>(  "lifecycle_manager_test_service_client" );
           nav2_lifecycle_manager::LifecycleManagerClient client(  "lifecycle_manager_test",   node );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::TIMEOUT,  
           client.is_active(  std::chrono::nanoseconds(  1000 ) ) );
           EXPECT_TRUE(  client.startup(   ) );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::ACTIVE,  
           client.is_active(  std::chrono::nanoseconds(  1000000000 ) ) );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::ACTIVE,  
           client.is_active(   ) );
           EXPECT_TRUE(  client.pause(   ) );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::INACTIVE,  
           client.is_active(  std::chrono::nanoseconds(  1000000000 ) ) );
           EXPECT_TRUE(  client.resume(   ) );
           EXPECT_TRUE(  client.reset(   ) );
           EXPECT_TRUE(  client.shutdown(   ) );
          }
          
          TEST(  LifecycleClientTest,   WithoutFixture )
          {
           auto node = std::make_shared<rclcpp::Node>(  "lifecycle_manager_test_service_client" );
           nav2_lifecycle_manager::LifecycleManagerClient client(  "lifecycle_manager_test",   node );
           EXPECT_EQ(  
           nav2_lifecycle_manager::SystemStatus::TIMEOUT,  
           client.is_active(  std::chrono::nanoseconds(  1000 ) ) );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
          #define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
          
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_msgs/msg/costmap_filter_info.hpp"
          
          namespace nav2_map_server
          {
          
      27  class CostmapFilterInfoServer : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief Constructor for the nav2_map_server::CostmapFilterInfoServer
           */
      33   CostmapFilterInfoServer(   );
           /**
           * @brief Destructor for the nav2_map_server::CostmapFilterInfoServer
           */
      37   ~CostmapFilterInfoServer(   );
          
          protected:
           /**
           * @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Publishes a CostmapFilterInfo message
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Deactivates publisher
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Resets publisher
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in Shutdown state
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
          private:
           rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
          
           std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg_;
          }; // CostmapFilterInfoServer
          
          } // namespace nav2_map_server
          
          #endif // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_mode.hpp

       1  // Copyright (  c ) 2019 Rover Robotics
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_MAP_SERVER__MAP_MODE_HPP_
          #define NAV2_MAP_SERVER__MAP_MODE_HPP_
          
          #include <string>
          #include <vector>
          namespace nav2_map_server
          {
          /**
           * @enum nav2_map_server::MapMode
           * @brief Describes the relation between image pixel values and map occupancy
           * status (  0-100; -1 ). Lightness refers to the mean of a given pixel's RGB
           * channels on a scale from 0 to 255.
           */
      28  enum class MapMode
          {
           /**
           * Together with associated threshold values (  occupied and free ):
           * lightness >= occupied threshold - Occupied (  100 )
           * ... (  anything in between ) - Unknown (  -1 )
           * lightness <= free threshold - Free (  0 )
           */
           Trinary,  
           /**
           * Together with associated threshold values (  occupied and free ):
           * alpha < 1.0 - Unknown (  -1 )
           * lightness >= occ_th - Occupied (  100 )
           * ... (  linearly interpolate to )
           * lightness <= free_th - Free (  0 )
           */
           Scale,  
           /**
           * Lightness = 0 - Free (  0 )
           * ... (  linearly interpolate to )
           * Lightness = 100 - Occupied (  100 )
           * Lightness >= 101 - Unknown
           */
           Raw,  
          };
          
          /**
           * @brief Convert a MapMode enum to the name of the map mode
           * @param map_mode Mode for the map
           * @return String identifier of the given map mode
           * @throw std::invalid_argument if the given value is not a defined map mode
           */
      60  const char * map_mode_to_string(  MapMode map_mode );
          
          /**
           * @brief Convert the name of a map mode to a MapMode enum
           * @param map_mode_name Name of the map mode
           * @throw std::invalid_argument if the name does not name a map mode
           * @return map mode corresponding to the string
           */
      68  MapMode map_mode_from_string(  std::string map_mode_name );
          } // namespace nav2_map_server
          
          #endif // NAV2_MAP_SERVER__MAP_MODE_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_saver.hpp

          // Copyright (  c ) 2020 Samsung Research Russia
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_MAP_SERVER__MAP_SAVER_HPP_
          #define NAV2_MAP_SERVER__MAP_SAVER_HPP_
          
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_msgs/srv/save_map.hpp"
          
          #include "map_io.hpp"
          
          namespace nav2_map_server
          {
          
          /**
           * @class nav2_map_server::MapSaver
           * @brief A class that provides map saving methods and services
           */
      35  class MapSaver : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief Constructor for the nav2_map_server::MapSaver
           * @param options Additional options to control creation of the node.
           */
      42   explicit MapSaver(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
          
           /**
           * @brief Destructor for the nav2_map_server::MapServer
           */
      47   ~MapSaver(   );
          
           /**
           * @brief Read a message from incoming map topic and save map to a file
           * @param map_topic Incoming map topic name
           * @param save_parameters Map saving parameters.
           * @return true of false
           */
      55   bool saveMapTopicToFile(  
      56   const std::string & map_topic,  
      57   const SaveParameters & save_parameters );
          
           /**
           * @brief Sets up map saving service
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when node switched to active state
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when node switched to inactive state
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when it is required node clean-up
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in Shutdown state
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
          protected:
           /**
           * @brief Map saving service callback
           * @param request_header Service request header
           * @param request Service request
           * @param response Service response
           */
           void saveMapCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,  
           std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response );
          
           // The timeout for saving the map in service
           std::shared_ptr<rclcpp::Duration> save_map_timeout_;
           // Default values for map thresholds
           double free_thresh_default_;
           double occupied_thresh_default_;
           // param for handling QoS configuration
           bool map_subscribe_transient_local_;
          
           // The name of the service for saving a map from topic
           const std::string save_map_service_name_{"save_map"};
           // A service to save the map to a file at run time (  SaveMap )
           rclcpp::Service<nav2_msgs::srv::SaveMap>::SharedPtr save_map_service_;
          };
          
          } // namespace nav2_map_server
          
          #endif // NAV2_MAP_SERVER__MAP_SAVER_HPP_

navigation2/nav2_map_server/include/nav2_map_server/map_server.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_
          #define NAV2_MAP_SERVER__MAP_SERVER_HPP_
          
          #include <string>
          #include <memory>
          #include <functional>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav_msgs/srv/get_map.hpp"
          #include "nav2_msgs/srv/load_map.hpp"
          
          namespace nav2_map_server
          {
          
          /**
           * @class nav2_map_server::MapServer
           * @brief Parses the map yaml file and creates a service and a publisher that
           * provides occupancy grid
           */
      36  class MapServer : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief A constructor for nav2_map_server::MapServer
           * @param options Additional options to control creation of the node.
           */
      43   explicit MapServer(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
          
           /**
           * @brief A Destructor for nav2_map_server::MapServer
           */
      48   ~MapServer(   );
          
          protected:
           /**
           * @brief Sets up required params and services. Loads map and its parameters from the file
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Start publishing the map using the latched topic
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Stops publishing the latched topic
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Resets the member variables
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in Shutdown state
           * @param state Lifecycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Load the map YAML,   image from map file name and
           * generate output response containing an OccupancyGrid.
           * Update msg_ class variable.
           * @param yaml_file name of input YAML file
           * @param response Output response with loaded OccupancyGrid map
           * @return true or false
           */
           bool loadMapResponseFromYaml(  
           const std::string & yaml_file,  
           std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response );
          
           /**
           * @brief Method correcting msg_ header when it belongs to instantiated object
           */
           void updateMsgHeader(   );
          
           /**
           * @brief Map getting service callback
           * @param request_header Service request header
           * @param request Service request
           * @param response Service response
           */
           void getMapCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,  
           std::shared_ptr<nav_msgs::srv::GetMap::Response> response );
          
           /**
           * @brief Map loading service callback
           * @param request_header Service request header
           * @param request Service request
           * @param response Service response
           */
           void loadMapCallback(  
           const std::shared_ptr<rmw_request_id_t> request_header,  
           const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,  
           std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response );
          
           // The name of the service for getting a map
           const std::string service_name_{"map"};
          
           // The name of the service for loading a map
           const std::string load_map_service_name_{"load_map"};
          
           // A service to provide the occupancy grid (  GetMap ) and the message to return
           rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
          
           // A service to load the occupancy grid from file at run time (  LoadMap )
           rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
          
           // A topic on which the occupancy grid will be published
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
          
           // The frame ID used in the returned OccupancyGrid message
           std::string frame_id_;
          
           // The message to publish on the occupancy grid topic
           nav_msgs::msg::OccupancyGrid msg_;
          
           // true if msg_ was initialized
           bool map_available_;
          };
          
          } // namespace nav2_map_server
          
          #endif // NAV2_MAP_SERVER__MAP_SERVER_HPP_

navigation2/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          // TODO(  AlexeyMerzlyakov ): This dummy info publisher should be removed
          // after Semantic Map Server having the same functionality will be developed.
          
          #include "nav2_map_server/costmap_filter_info_server.hpp"
          
          #include <string>
          #include <memory>
          #include <utility>
          
          namespace nav2_map_server
          {
          
      27  CostmapFilterInfoServer::CostmapFilterInfoServer(   )
          : nav2_util::LifecycleNode(  "costmap_filter_info_server" )
          {
           declare_parameter(  "filter_info_topic",   "costmap_filter_info" );
           declare_parameter(  "type",   0 );
           declare_parameter(  "mask_topic",   "filter_mask" );
           declare_parameter(  "base",   0.0 );
           declare_parameter(  "multiplier",   1.0 );
          }
          
      37  CostmapFilterInfoServer::~CostmapFilterInfoServer(   )
          {
          }
          
          nav2_util::CallbackReturn
      42  CostmapFilterInfoServer::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           std::string filter_info_topic = get_parameter(  "filter_info_topic" ).as_string(   );
          
           publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(  
           filter_info_topic,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
          
           msg_ = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>(   );
           msg_->header.frame_id = "";
           msg_->header.stamp = now(   );
           msg_->type = get_parameter(  "type" ).as_int(   );
           msg_->filter_mask_topic = get_parameter(  "mask_topic" ).as_string(   );
           msg_->base = static_cast<float>(  get_parameter(  "base" ).as_double(   ) );
           msg_->multiplier = static_cast<float>(  get_parameter(  "multiplier" ).as_double(   ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
      63  CostmapFilterInfoServer::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           publisher_->on_activate(   );
           publisher_->publish(  std::move(  msg_ ) );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
      77  CostmapFilterInfoServer::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           publisher_->on_deactivate(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
      90  CostmapFilterInfoServer::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           publisher_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     100  CostmapFilterInfoServer::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          } // namespace nav2_map_server

navigation2/nav2_map_server/src/costmap_filter_info/main.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_map_server/costmap_filter_info_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char * argv[] )
          {
           auto logger = rclcpp::get_logger(  "costmap_filter_info_server" );
          
           RCLCPP_INFO(  logger,   "This is costmap filter info publisher" );
          
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_map_server::CostmapFilterInfoServer>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_map_server/src/map_io.cpp

       1  /* Copyright 2019 Rover Robotics
           * Copyright 2010 Brian Gerkey
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           *
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav2_map_server/map_io.hpp"
          
          #ifndef _WIN32
          #include <libgen.h>
          #endif
          #include <iostream>
          #include <string>
          #include <vector>
          #include <fstream>
          #include <stdexcept>
          
          #include "Magick++.h"
          #include "nav2_util/geometry_utils.hpp"
          
          #include "yaml-cpp/yaml.h"
          #include "tf2/LinearMath/Matrix3x3.h"
          #include "tf2/LinearMath/Quaternion.h"
          #include "nav2_util/occ_grid_values.hpp"
          
          #ifdef _WIN32
          // https://github.com/rtv/Stage/blob/master/replace/dirname.c
          static
      54  char * dirname(  char * path )
          {
           static const char dot[] = ".";
           char * last_slash;
          
           if (  path == NULL ) {
           return path;
           }
          
           /* Replace all "\" with "/" */
           char * c = path;
           while (  *c != '\0' ) {
           if (  *c == '\\' ) {*c = '/';}
           ++c;
           }
          
           /* Find last '/'. */
           last_slash = path != NULL ? strrchr(  path,   '/' ) : NULL;
          
           if (  last_slash != NULL && last_slash == path ) {
           /* The last slash is the first character in the string. We have to
           return "/". */
           ++last_slash;
           } else if (  last_slash != NULL && last_slash[1] == '\0' ) {
           /* The '/' is the last character,   we have to look further. */
           last_slash = reinterpret_cast<char *>(  memchr(  path,   last_slash - path,   '/' ) );
           }
          
           if (  last_slash != NULL ) {
           /* Terminate the path. */
           last_slash[0] = '\0';
           } else {
           /* This assignment is ill-designed but the XPG specs require to
           return a string containing "." in any case no directory part is
           found and so a static and constant string is required. */
           path = reinterpret_cast<char *>(  dot );
           }
          
           return path;
          }
          #endif
          
          namespace nav2_map_server
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
          // === Map input part ===
          
          /// Get the given subnode value.
          /// The only reason this function exists is to wrap the exceptions in slightly nicer error messages,  
          /// including the name of the failed key
          /// @throw YAML::Exception
          template<typename T>
     107  T yaml_get_value(  const YAML::Node & node,   const std::string & key )
          {
           try {
           return node[key].as<T>(   );
           } catch (  YAML::Exception & e ) {
           std::stringstream ss;
           ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg;
           throw YAML::Exception(  e.mark,   ss.str(   ) );
           }
          }
          
     118  LoadParameters loadMapYaml(  const std::string & yaml_filename )
          {
           YAML::Node doc = YAML::LoadFile(  yaml_filename );
           LoadParameters load_parameters;
          
           auto image_file_name = yaml_get_value<std::string>(  doc,   "image" );
           if (  image_file_name.empty(   ) ) {
           throw YAML::Exception(  doc["image"].Mark(   ),   "The image tag was empty." );
           }
           if (  image_file_name[0] != '/' ) {
           // dirname takes a mutable char *,   so we copy into a vector
           std::vector<char> fname_copy(  yaml_filename.begin(   ),   yaml_filename.end(   ) );
           fname_copy.push_back(  '\0' );
           image_file_name = std::string(  dirname(  fname_copy.data(   ) ) ) + '/' + image_file_name;
           }
           load_parameters.image_file_name = image_file_name;
          
           load_parameters.resolution = yaml_get_value<double>(  doc,   "resolution" );
           load_parameters.origin = yaml_get_value<std::vector<double>>(  doc,   "origin" );
           if (  load_parameters.origin.size(   ) != 3 ) {
           throw YAML::Exception(  
           doc["origin"].Mark(   ),   "value of the 'origin' tag should have 3 elements,   not " +
           std::to_string(  load_parameters.origin.size(   ) ) );
           }
          
           load_parameters.free_thresh = yaml_get_value<double>(  doc,   "free_thresh" );
           load_parameters.occupied_thresh = yaml_get_value<double>(  doc,   "occupied_thresh" );
          
           auto map_mode_node = doc["mode"];
           if (  !map_mode_node.IsDefined(   ) ) {
           load_parameters.mode = MapMode::Trinary;
           } else {
           load_parameters.mode = map_mode_from_string(  map_mode_node.as<std::string>(   ) );
           }
          
           try {
           load_parameters.negate = yaml_get_value<int>(  doc,   "negate" );
           } catch (  YAML::Exception & ) {
           load_parameters.negate = yaml_get_value<bool>(  doc,   "negate" );
           }
          
           std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl;
           std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl;
           std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl;
           std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl;
           std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl;
           std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh <<
           std::endl;
           std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(  load_parameters.mode ) << std::endl;
           std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT
          
           return load_parameters;
          }
          
     172  void loadMapFromFile(  
           const LoadParameters & load_parameters,  
           nav_msgs::msg::OccupancyGrid & map )
          {
           Magick::InitializeMagick(  nullptr );
           nav_msgs::msg::OccupancyGrid msg;
          
           std::cout << "[INFO] [map_io]: Loading image_file: " <<
           load_parameters.image_file_name << std::endl;
           Magick::Image img(  load_parameters.image_file_name );
          
           // Copy the image data into the map structure
           msg.info.width = img.size(   ).width(   );
           msg.info.height = img.size(   ).height(   );
          
           msg.info.resolution = load_parameters.resolution;
           msg.info.origin.position.x = load_parameters.origin[0];
           msg.info.origin.position.y = load_parameters.origin[1];
           msg.info.origin.position.z = 0.0;
           msg.info.origin.orientation = orientationAroundZAxis(  load_parameters.origin[2] );
          
           // Allocate space to hold the data
           msg.data.resize(  msg.info.width * msg.info.height );
          
           // Copy pixel data into the map structure
           for (  size_t y = 0; y < msg.info.height; y++ ) {
           for (  size_t x = 0; x < msg.info.width; x++ ) {
           auto pixel = img.pixelColor(  x,   y );
          
           std::vector<Magick::Quantum> channels = {pixel.redQuantum(   ),   pixel.greenQuantum(   ),  
           pixel.blueQuantum(   )};
           if (  load_parameters.mode == MapMode::Trinary && img.matte(   ) ) {
           // To preserve existing behavior,   average in alpha with color channels in Trinary mode.
           // CAREFUL. alpha is inverted from what you might expect. High = transparent,   low = opaque
           channels.push_back(  MaxRGB - pixel.alphaQuantum(   ) );
           }
           double sum = 0;
           for (  auto c : channels ) {
           sum += c;
           }
           /// on a scale from 0.0 to 1.0 how bright is the pixel?
           double shade = Magick::ColorGray::scaleQuantumToDouble(  sum / channels.size(   ) );
          
           // If negate is true,   we consider blacker pixels free,   and whiter
           // pixels occupied. Otherwise,   it's vice versa.
           /// on a scale from 0.0 to 1.0,   how occupied is the map cell (  before thresholding )?
           double occ = (  load_parameters.negate ? shade : 1.0 - shade );
          
           int8_t map_cell;
           switch (  load_parameters.mode ) {
           case MapMode::Trinary:
           if (  load_parameters.occupied_thresh < occ ) {
           map_cell = nav2_util::OCC_GRID_OCCUPIED;
           } else if (  occ < load_parameters.free_thresh ) {
           map_cell = nav2_util::OCC_GRID_FREE;
           } else {
           map_cell = nav2_util::OCC_GRID_UNKNOWN;
           }
           break;
           case MapMode::Scale:
           if (  pixel.alphaQuantum(   ) != OpaqueOpacity ) {
           map_cell = nav2_util::OCC_GRID_UNKNOWN;
           } else if (  load_parameters.occupied_thresh < occ ) {
           map_cell = nav2_util::OCC_GRID_OCCUPIED;
           } else if (  occ < load_parameters.free_thresh ) {
           map_cell = nav2_util::OCC_GRID_FREE;
           } else {
           map_cell = std::rint(  
           (  occ - load_parameters.free_thresh ) /
           (  load_parameters.occupied_thresh - load_parameters.free_thresh ) * 100.0 );
           }
           break;
           case MapMode::Raw: {
           double occ_percent = std::round(  shade * 255 );
           if (  nav2_util::OCC_GRID_FREE <= occ_percent &&
           occ_percent <= nav2_util::OCC_GRID_OCCUPIED )
           {
           map_cell = static_cast<int8_t>(  occ_percent );
           } else {
           map_cell = nav2_util::OCC_GRID_UNKNOWN;
           }
           break;
           }
           default:
           throw std::runtime_error(  "Invalid map mode" );
           }
           msg.data[msg.info.width * (  msg.info.height - y - 1 ) + x] = map_cell;
           }
           }
          
           // Since loadMapFromFile(   ) does not belong to any node,   publishing in a system time.
           rclcpp::Clock clock(  RCL_SYSTEM_TIME );
           msg.info.map_load_time = clock.now(   );
           msg.header.frame_id = "map";
           msg.header.stamp = clock.now(   );
          
           std::cout <<
           "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width <<
           " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl;
          
           map = msg;
          }
          
     275  LOAD_MAP_STATUS loadMapFromYaml(  
           const std::string & yaml_file,  
           nav_msgs::msg::OccupancyGrid & map )
          {
           if (  yaml_file.empty(   ) ) {
           std::cerr << "[ERROR] [map_io]: YAML file name is empty,   can't load!" << std::endl;
           return MAP_DOES_NOT_EXIST;
           }
           std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl;
           LoadParameters load_parameters;
           try {
           load_parameters = loadMapYaml(  yaml_file );
           } catch (  YAML::Exception & e ) {
           std::cerr <<
           "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (  " <<
           e.mark.line << ":" << e.mark.column << " ) for reason: " << e.what(   ) << std::endl;
           return INVALID_MAP_METADATA;
           } catch (  std::exception & e ) {
           std::cerr <<
           "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file <<
           " for reason: " << e.what(   ) << std::endl;
           return INVALID_MAP_METADATA;
           }
          
           try {
           loadMapFromFile(  load_parameters,   map );
           } catch (  std::exception & e ) {
           std::cerr <<
           "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name <<
           " for reason: " << e.what(   ) << std::endl;
           return INVALID_MAP_DATA;
           }
          
           return LOAD_MAP_SUCCESS;
          }
          
          // === Map output part ===
          
          /**
           * @brief Checks map saving parameters for consistency
           * @param save_parameters Map saving parameters.
           * NOTE: save_parameters could be updated during function execution.
           * @throw std::exception in case of inconsistent parameters
           */
     319  void checkSaveParameters(  SaveParameters & save_parameters )
          {
           // Magick must me initialized before any activity with images
           Magick::InitializeMagick(  nullptr );
          
           // Checking map file name
           if (  save_parameters.map_file_name == "" ) {
           rclcpp::Clock clock(  RCL_SYSTEM_TIME );
           save_parameters.map_file_name = "map_" +
           std::to_string(  static_cast<int>(  clock.now(   ).seconds(   ) ) );
           std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " <<
           save_parameters.map_file_name << " file" << std::endl;
           }
          
           // Checking thresholds
           if (  save_parameters.occupied_thresh == 0.0 ) {
           save_parameters.occupied_thresh = 0.65;
           std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " <<
           save_parameters.occupied_thresh << std::endl;
           }
           if (  save_parameters.free_thresh == 0.0 ) {
           save_parameters.free_thresh = 0.25;
           std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " <<
           save_parameters.free_thresh << std::endl;
           }
           if (  1.0 < save_parameters.occupied_thresh ) {
           std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl;
           throw std::runtime_error(  "Incorrect thresholds" );
           }
           if (  save_parameters.free_thresh < 0.0 ) {
           std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl;
           throw std::runtime_error(  "Incorrect thresholds" );
           }
           if (  save_parameters.occupied_thresh <= save_parameters.free_thresh ) {
           std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" <<
           std::endl;
           throw std::runtime_error(  "Incorrect thresholds" );
           }
          
           // Checking image format
           if (  save_parameters.image_format == "" ) {
           save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm";
           std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " <<
           save_parameters.image_format << std::endl;
           }
          
           std::transform(  
           save_parameters.image_format.begin(   ),  
           save_parameters.image_format.end(   ),  
           save_parameters.image_format.begin(   ),  
           [](  unsigned char c ) {return std::tolower(  c );} );
          
           const std::vector<std::string> BLESSED_FORMATS{"bmp",   "pgm",   "png"};
           if (  
           std::find(  BLESSED_FORMATS.begin(   ),   BLESSED_FORMATS.end(   ),   save_parameters.image_format ) ==
           BLESSED_FORMATS.end(   ) )
           {
           std::stringstream ss;
           bool first = true;
           for (  auto & format_name : BLESSED_FORMATS ) {
           if (  !first ) {
           ss << ",   ";
           }
           ss << "'" << format_name << "'";
           first = false;
           }
           std::cout <<
           "[WARN] [map_io]: Requested image format '" << save_parameters.image_format <<
           "' is not one of the recommended formats: " << ss.str(   ) << std::endl;
           }
           const std::string FALLBACK_FORMAT = "png";
          
           try {
           Magick::CoderInfo info(  save_parameters.image_format );
           if (  !info.isWritable(   ) ) {
           std::cout <<
           "[WARN] [map_io]: Format '" << save_parameters.image_format <<
           "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl;
           save_parameters.image_format = FALLBACK_FORMAT;
           }
           } catch (  Magick::ErrorOption & e ) {
           std::cout <<
           "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" <<
           FALLBACK_FORMAT << "' instead:" << std::endl << e.what(   ) << std::endl;
           save_parameters.image_format = FALLBACK_FORMAT;
           }
          
           // Checking map mode
           if (  
           save_parameters.mode == MapMode::Scale &&
           (  save_parameters.image_format == "pgm" ||
           save_parameters.image_format == "jpg" ||
           save_parameters.image_format == "jpeg" ) )
           {
           std::cout <<
           "[WARN] [map_io]: Map mode 'scale' requires transparency,   but format '" <<
           save_parameters.image_format <<
           "' does not support it. Consider switching image format to 'png'." << std::endl;
           }
          }
          
          /**
           * @brief Tries to write map data into a file
           * @param map Occupancy grid data
           * @param save_parameters Map saving parameters
           * @throw std::expection in case of problem
           */
     426  void tryWriteMapToFile(  
           const nav_msgs::msg::OccupancyGrid & map,  
           const SaveParameters & save_parameters )
          {
           std::cout <<
           "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " <<
           map.info.resolution << " m/pix" << std::endl;
          
           std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format;
           {
           // should never see this color,   so the initialization value is just for debugging
           Magick::Image image(  {map.info.width,   map.info.height},   "red" );
          
           // In scale mode,   we need the alpha (  matte ) channel. Else,   we don't.
           // NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with
           // Magick::GreyscaleMatte,   so we use TrueColorMatte instead.
           image.type(  
           save_parameters.mode == MapMode::Scale ?
           Magick::TrueColorMatteType : Magick::GrayscaleType );
          
           // Since we only need to support 100 different pixel levels,   8 bits is fine
           image.depth(  8 );
          
           int free_thresh_int = std::rint(  save_parameters.free_thresh * 100.0 );
           int occupied_thresh_int = std::rint(  save_parameters.occupied_thresh * 100.0 );
          
           for (  size_t y = 0; y < map.info.height; y++ ) {
           for (  size_t x = 0; x < map.info.width; x++ ) {
           int8_t map_cell = map.data[map.info.width * (  map.info.height - y - 1 ) + x];
          
           Magick::Color pixel;
          
           switch (  save_parameters.mode ) {
           case MapMode::Trinary:
           if (  map_cell < 0 || 100 < map_cell ) {
           pixel = Magick::ColorGray(  205 / 255.0 );
           } else if (  map_cell <= free_thresh_int ) {
           pixel = Magick::ColorGray(  254 / 255.0 );
           } else if (  occupied_thresh_int <= map_cell ) {
           pixel = Magick::ColorGray(  0 / 255.0 );
           } else {
           pixel = Magick::ColorGray(  205 / 255.0 );
           }
           break;
           case MapMode::Scale:
           if (  map_cell < 0 || 100 < map_cell ) {
           pixel = Magick::ColorGray{0.5};
           pixel.alphaQuantum(  TransparentOpacity );
           } else {
           pixel = Magick::ColorGray{(  100.0 - map_cell ) / 100.0};
           }
           break;
           case MapMode::Raw:
           Magick::Quantum q;
           if (  map_cell < 0 || 100 < map_cell ) {
           q = MaxRGB;
           } else {
           q = map_cell / 255.0 * MaxRGB;
           }
           pixel = Magick::Color(  q,   q,   q );
           break;
           default:
           std::cerr << "[ERROR] [map_io]: Map mode should be Trinary,   Scale or Raw" << std::endl;
           throw std::runtime_error(  "Invalid map mode" );
           }
           image.pixelColor(  x,   y,   pixel );
           }
           }
          
           std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl;
           image.write(  mapdatafile );
           }
          
           std::string mapmetadatafile = save_parameters.map_file_name + ".yaml";
           {
           std::ofstream yaml(  mapmetadatafile );
          
           geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation;
           tf2::Matrix3x3 mat(  tf2::Quaternion(  orientation.x,   orientation.y,   orientation.z,   orientation.w ) );
           double yaw,   pitch,   roll;
           mat.getEulerYPR(  yaw,   pitch,   roll );
          
           YAML::Emitter e;
           e << YAML::Precision(  3 );
           e << YAML::BeginMap;
           e << YAML::Key << "image" << YAML::Value << mapdatafile;
           e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(  save_parameters.mode );
           e << YAML::Key << "resolution" << YAML::Value << map.info.resolution;
           e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x <<
           map.info.origin.position.y << yaw << YAML::EndSeq;
           e << YAML::Key << "negate" << YAML::Value << 0;
           e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh;
           e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
          
           if (  !e.good(   ) ) {
           std::cout <<
           "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError(   ) <<
           ". The map metadata may be invalid." << std::endl;
           }
          
           std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl;
           std::ofstream(  mapmetadatafile ) << e.c_str(   );
           }
           std::cout << "[INFO] [map_io]: Map saved" << std::endl;
          }
          
     532  bool saveMapToFile(  
           const nav_msgs::msg::OccupancyGrid & map,  
           const SaveParameters & save_parameters )
          {
           // Local copy of SaveParameters that might be modified by checkSaveParameters(   )
           SaveParameters save_parameters_loc = save_parameters;
          
           try {
           // Checking map parameters for consistency
           checkSaveParameters(  save_parameters_loc );
          
           tryWriteMapToFile(  map,   save_parameters_loc );
           } catch (  std::exception & e ) {
           std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what(   ) << std::endl;
           return false;
           }
           return true;
          }
          
          } // namespace nav2_map_server

navigation2/nav2_map_server/src/map_mode.cpp

       1  // Copyright 2019 Rover Robotics
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_map_server/map_mode.hpp"
          
          #include <stdexcept>
          #include <string>
          
          namespace nav2_map_server
          {
      22  const char * map_mode_to_string(  MapMode map_mode )
          {
           switch (  map_mode ) {
           case MapMode::Trinary:
           return "trinary";
           case MapMode::Scale:
           return "scale";
           case MapMode::Raw:
           return "raw";
           default:
           throw std::invalid_argument(  "map_mode" );
           }
          }
          
      36  MapMode map_mode_from_string(  std::string map_mode_name )
          {
           for (  auto & c : map_mode_name ) {
           c = tolower(  c );
           }
          
           if (  map_mode_name == "scale" ) {
           return MapMode::Scale;
           } else if (  map_mode_name == "raw" ) {
           return MapMode::Raw;
           } else if (  map_mode_name == "trinary" ) {
           return MapMode::Trinary;
           } else {
           throw std::invalid_argument(  "map_mode_name" );
           }
          }
          } // namespace nav2_map_server

navigation2/nav2_map_server/src/map_saver/main_server.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <stdexcept>
          #include <string>
          
          #include "nav2_map_server/map_saver.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      23  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
          
           auto logger = rclcpp::get_logger(  "map_saver_server" );
           auto service_node = std::make_shared<nav2_map_server::MapSaver>(   );
           rclcpp::spin(  service_node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
           return 0;
          }

navigation2/nav2_map_server/src/map_saver/map_saver.cpp

       1  /*
           * Copyright (  c ) 2020 Samsung Research Russia
           * Copyright 2019 Rover Robotics
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the <ORGANIZATION> nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav2_map_server/map_saver.hpp"
          
          #include <string>
          #include <memory>
          #include <stdexcept>
          #include <functional>
          #include <mutex>
          
          using namespace std::placeholders;
          
          namespace nav2_map_server
          {
      44  MapSaver::MapSaver(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "map_saver",   "",   options )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           // Declare the node parameters
           declare_parameter(  "save_map_timeout",   2.0 );
           declare_parameter(  "free_thresh_default",   0.25 );
           declare_parameter(  "occupied_thresh_default",   0.65 );
           declare_parameter(  "map_subscribe_transient_local",   true );
          }
          
      56  MapSaver::~MapSaver(   )
          {
          }
          
          nav2_util::CallbackReturn
      61  MapSaver::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           // Make name prefix for services
           const std::string service_prefix = get_name(   ) + std::string(  "/" );
          
           save_map_timeout_ = std::make_shared<rclcpp::Duration>(  
           rclcpp::Duration::from_seconds(  get_parameter(  "save_map_timeout" ).as_double(   ) ) );
           free_thresh_default_ = get_parameter(  "free_thresh_default" ).as_double(   );
           occupied_thresh_default_ = get_parameter(  "occupied_thresh_default" ).as_double(   );
           map_subscribe_transient_local_ = get_parameter(  "map_subscribe_transient_local" ).as_bool(   );
          
           // Create a service that saves the occupancy grid from map topic to a file
           save_map_service_ = create_service<nav2_msgs::srv::SaveMap>(  
           service_prefix + save_map_service_name_,  
           std::bind(  &MapSaver::saveMapCallback,   this,   _1,   _2,   _3 ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
      83  MapSaver::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
      94  MapSaver::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     105  MapSaver::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           save_map_service_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     115  MapSaver::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
     121  void MapSaver::saveMapCallback(  
     122   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     123   const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,  
     124   std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response )
          {
           // Set input arguments and call saveMapTopicToFile(   )
           SaveParameters save_parameters;
           save_parameters.map_file_name = request->map_url;
           save_parameters.image_format = request->image_format;
           save_parameters.free_thresh = request->free_thresh;
           save_parameters.occupied_thresh = request->occupied_thresh;
           try {
           save_parameters.mode = map_mode_from_string(  request->map_mode );
           } catch (  std::invalid_argument & ) {
           save_parameters.mode = MapMode::Trinary;
           RCLCPP_WARN(  
           get_logger(   ),   "Map mode parameter not recognized: '%s',   using default value (  trinary )",  
           request->map_mode.c_str(   ) );
           }
          
           response->result = saveMapTopicToFile(  request->map_topic,   save_parameters );
          }
          
     144  bool MapSaver::saveMapTopicToFile(  
     145   const std::string & map_topic,  
     146   const SaveParameters & save_parameters )
          {
           // Local copies of map_topic and save_parameters that could be changed
           std::string map_topic_loc = map_topic;
           SaveParameters save_parameters_loc = save_parameters;
          
           RCLCPP_INFO(  
           get_logger(   ),   "Saving map from \'%s\' topic to \'%s\' file",  
           map_topic_loc.c_str(   ),   save_parameters_loc.map_file_name.c_str(   ) );
          
           try {
           // Correct map_topic_loc if necessary
           if (  map_topic_loc == "" ) {
           map_topic_loc = "map";
           RCLCPP_WARN(  
           get_logger(   ),   "Map topic unspecified. Map messages will be read from \'%s\' topic",  
           map_topic_loc.c_str(   ) );
           }
          
           // Set default for MapSaver node thresholds parameters
           if (  save_parameters_loc.free_thresh == 0.0 ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Free threshold unspecified. Setting it to default value: %f",  
           free_thresh_default_ );
           save_parameters_loc.free_thresh = free_thresh_default_;
           }
           if (  save_parameters_loc.occupied_thresh == 0.0 ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Occupied threshold unspecified. Setting it to default value: %f",  
           occupied_thresh_default_ );
           save_parameters_loc.occupied_thresh = occupied_thresh_default_;
           }
          
           std::promise<nav_msgs::msg::OccupancyGrid::SharedPtr> prom;
           std::future<nav_msgs::msg::OccupancyGrid::SharedPtr> future_result = prom.get_future(   );
           // A callback function that receives map message from subscribed topic
           auto mapCallback = [&prom](  
           const nav_msgs::msg::OccupancyGrid::SharedPtr msg ) -> void {
           prom.set_value(  msg );
           };
          
           rclcpp::QoS map_qos(  10 ); // initialize to default
           if (  map_subscribe_transient_local_ ) {
           map_qos.transient_local(   );
           map_qos.reliable(   );
           map_qos.keep_last(  1 );
           }
          
           // Create new CallbackGroup for map_sub
           auto callback_group = create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
          
           auto option = rclcpp::SubscriptionOptions(   );
           option.callback_group = callback_group;
           auto map_sub = create_subscription<nav_msgs::msg::OccupancyGrid>(  
           map_topic_loc,   map_qos,   mapCallback,   option );
          
           // Create SingleThreadedExecutor to spin map_sub in callback_group
           rclcpp::executors::SingleThreadedExecutor executor;
           executor.add_callback_group(  callback_group,   get_node_base_interface(   ) );
           // Spin until map message received
           auto timeout = save_map_timeout_->to_chrono<std::chrono::nanoseconds>(   );
           auto status = executor.spin_until_future_complete(  future_result,   timeout );
           if (  status != rclcpp::FutureReturnCode::SUCCESS ) {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to spin map subscription" );
           return false;
           }
           // map_sub is no more needed
           map_sub.reset(   );
           // Map message received. Saving it to file
           nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get(   );
           if (  saveMapToFile(  *map_msg,   save_parameters_loc ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Map saved successfully" );
           return true;
           } else {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to save the map" );
           return false;
           }
           } catch (  std::exception & e ) {
           RCLCPP_ERROR(  get_logger(   ),   "Failed to save the map: %s",   e.what(   ) );
           return false;
           }
          
           return false;
          }
          
          } // namespace nav2_map_server
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
     242  RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_map_server::MapSaver )

navigation2/nav2_map_server/src/map_server/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <stdexcept>
          #include <string>
          
          #include "nav2_map_server/map_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      22  int main(  int argc,   char ** argv )
          {
           std::string node_name(  "map_server" );
          
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_map_server::MapServer>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          }

navigation2/nav2_map_server/src/map_server/map_server.cpp

       1  /* Copyright (  c ) 2018 Intel Corporation
           *
           * Licensed under the Apache License,   Version 2.0 (  the "License" );
           * you may not use this file except in compliance with the License.
           * You may obtain a copy of the License at
           *
           * http://www.apache.org/licenses/LICENSE-2.0
           *
           * Unless required by applicable law or agreed to in writing,   software
           * distributed under the License is distributed on an "AS IS" BASIS,  
           * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
           * See the License for the specific language governing permissions and
           * limitations under the License.
           */
          
          /* Copyright 2019 Rover Robotics
           * Copyright 2010 Brian Gerkey
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           *
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          #include "nav2_map_server/map_server.hpp"
          
          #include <string>
          #include <memory>
          #include <fstream>
          #include <stdexcept>
          #include <utility>
          
          #include "yaml-cpp/yaml.h"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "nav2_map_server/map_io.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::placeholders;
          
          namespace nav2_map_server
          {
          
      65  MapServer::MapServer(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "map_server",   "",   options ),   map_available_(  false )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           // Declare the node parameters
           declare_parameter(  "yaml_filename",   rclcpp::PARAMETER_STRING );
           declare_parameter(  "topic_name",   "map" );
           declare_parameter(  "frame_id",   "map" );
          }
          
      76  MapServer::~MapServer(   )
          {
          }
          
          nav2_util::CallbackReturn
      81  MapServer::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           // Get the name of the YAML file to use (  can be empty if no initial map should be used )
           std::string yaml_filename = get_parameter(  "yaml_filename" ).as_string(   );
           std::string topic_name = get_parameter(  "topic_name" ).as_string(   );
           frame_id_ = get_parameter(  "frame_id" ).as_string(   );
          
           // only try to load map if parameter was set
           if (  !yaml_filename.empty(   ) ) {
           // Shared pointer to LoadMap::Response is also should be initialized
           // in order to avoid null-pointer dereference
           std::shared_ptr<nav2_msgs::srv::LoadMap::Response> rsp =
           std::make_shared<nav2_msgs::srv::LoadMap::Response>(   );
          
           if (  !loadMapResponseFromYaml(  yaml_filename,   rsp ) ) {
           throw std::runtime_error(  "Failed to load map yaml file: " + yaml_filename );
           }
           } else {
           RCLCPP_INFO(  
           get_logger(   ),  
           "yaml-filename parameter is empty,   set map through '%s'-service",  
           load_map_service_name_.c_str(   ) );
           }
          
           // Make name prefix for services
           const std::string service_prefix = get_name(   ) + std::string(  "/" );
          
           // Create a service that provides the occupancy grid
           occ_service_ = create_service<nav_msgs::srv::GetMap>(  
           service_prefix + std::string(  service_name_ ),  
           std::bind(  &MapServer::getMapCallback,   this,   _1,   _2,   _3 ) );
          
           // Create a publisher using the QoS settings to emulate a ROS1 latched topic
           occ_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(  
           topic_name,  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
          
           // Create a service that loads the occupancy grid from a file
           load_map_service_ = create_service<nav2_msgs::srv::LoadMap>(  
           service_prefix + std::string(  load_map_service_name_ ),  
           std::bind(  &MapServer::loadMapCallback,   this,   _1,   _2,   _3 ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     129  MapServer::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           // Publish the map using the latched topic
           occ_pub_->on_activate(   );
           if (  map_available_ ) {
           auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(  msg_ );
           occ_pub_->publish(  std::move(  occ_grid ) );
           }
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     147  MapServer::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           occ_pub_->on_deactivate(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     160  MapServer::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           occ_pub_.reset(   );
           occ_service_.reset(   );
           load_map_service_.reset(   );
           map_available_ = false;
           msg_ = nav_msgs::msg::OccupancyGrid(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     174  MapServer::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
     180  void MapServer::getMapCallback(  
     181   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     182   const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,  
     183   std::shared_ptr<nav_msgs::srv::GetMap::Response> response )
          {
           // if not in ACTIVE state,   ignore request
           if (  get_current_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Received GetMap request but not in ACTIVE state,   ignoring!" );
           return;
           }
           RCLCPP_INFO(  get_logger(   ),   "Handling GetMap request" );
           response->map = msg_;
          }
          
     196  void MapServer::loadMapCallback(  
     197   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
     198   const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,  
     199   std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response )
          {
           // if not in ACTIVE state,   ignore request
           if (  get_current_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Received LoadMap request but not in ACTIVE state,   ignoring!" );
           response->result = response->RESULT_UNDEFINED_FAILURE;
           return;
           }
           RCLCPP_INFO(  get_logger(   ),   "Handling LoadMap request" );
           // Load from file
           if (  loadMapResponseFromYaml(  request->map_url,   response ) ) {
           auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(  msg_ );
           occ_pub_->publish(  std::move(  occ_grid ) ); // publish new map
           }
          }
          
     217  bool MapServer::loadMapResponseFromYaml(  
     218   const std::string & yaml_file,  
     219   std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response )
          {
           switch (  loadMapFromYaml(  yaml_file,   msg_ ) ) {
           case MAP_DOES_NOT_EXIST:
           response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST;
           return false;
           case INVALID_MAP_METADATA:
           response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
           return false;
           case INVALID_MAP_DATA:
           response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
           return false;
           case LOAD_MAP_SUCCESS:
           // Correcting msg_ header when it belongs to specific node
           updateMsgHeader(   );
          
           map_available_ = true;
           response->map = msg_;
           response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
           }
          
           return true;
          }
          
     243  void MapServer::updateMsgHeader(   )
          {
           msg_.info.map_load_time = now(   );
           msg_.header.frame_id = frame_id_;
           msg_.header.stamp = now(   );
          }
          
          } // namespace nav2_map_server
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
     257  RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_map_server::MapServer )

navigation2/nav2_map_server/test/component/test_map_saver_node.cpp

          // Copyright (  c ) 2020 Samsung Research Russia
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          
          #include <gtest/gtest.h>
          
          #include <string>
          #include <memory>
          #include <experimental/filesystem> // NOLINT
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "test_constants/test_constants.h"
          #include "nav2_map_server/map_saver.hpp"
          #include "nav2_util/lifecycle_service_client.hpp"
          #include "nav2_msgs/srv/save_map.hpp"
          
          #define TEST_DIR TEST_DIRECTORY
          
          using std::experimental::filesystem::path;
          using lifecycle_msgs::msg::Transition;
          using namespace nav2_map_server; // NOLINT
          
      36  class RclCppFixture
          {
          public:
      39   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      40   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          
          RclCppFixture g_rclcppfixture;
          
      45  class MapSaverTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = rclcpp::Node::make_shared(  "map_client_test" );
           lifecycle_client_ =
           std::make_shared<nav2_util::LifecycleServiceClient>(  "map_saver",   node_ );
           RCLCPP_INFO(  node_->get_logger(   ),   "Creating Test Node" );
          
           std::this_thread::sleep_for(  std::chrono::seconds(  5 ) ); // allow node to start up
           const std::chrono::seconds timeout(  5 );
           lifecycle_client_->change_state(  Transition::TRANSITION_CONFIGURE,   timeout );
           lifecycle_client_->change_state(  Transition::TRANSITION_ACTIVATE,   timeout );
           }
          
      61   static void TearDownTestCase(   )
           {
           lifecycle_client_->change_state(  Transition::TRANSITION_DEACTIVATE );
           lifecycle_client_->change_state(  Transition::TRANSITION_CLEANUP );
           lifecycle_client_.reset(   );
           node_.reset(   );
           }
          
           template<class T>
           typename T::Response::SharedPtr send_request(  
          
           rclcpp::Node::SharedPtr node,  
           typename rclcpp::Client<T>::SharedPtr client,  
           typename T::Request::SharedPtr request )
           {
           auto result = client->async_send_request(  request );
          
           // Wait for the result
           if (  rclcpp::spin_until_future_complete(  node,   result ) == rclcpp::FutureReturnCode::SUCCESS ) {
           return result.get(   );
           } else {
           return nullptr;
           }
           }
          
          protected:
           // Check that map_msg corresponds to reference pattern
           // Input: map_msg
           void verifyMapMsg(  const nav_msgs::msg::OccupancyGrid & map_msg )
           {
           ASSERT_FLOAT_EQ(  map_msg.info.resolution,   g_valid_image_res );
           ASSERT_EQ(  map_msg.info.width,   g_valid_image_width );
           ASSERT_EQ(  map_msg.info.height,   g_valid_image_height );
           for (  unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
           ASSERT_EQ(  g_valid_image_content[i],   map_msg.data[i] );
           }
           }
          
           static rclcpp::Node::SharedPtr node_;
           static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
          };
          
          
          rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
          std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
           nullptr;
          
          // Send map saving service request.
          // Load saved map and verify obtained OccupancyGrid.
          TEST_F(  MapSaverTestFixture,   SaveMap )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing SaveMap service" );
           auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::SaveMap>(  
           "/map_saver/save_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for save_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           // 1. Send valid save_map serivce request
           req->map_topic = "map";
           req->map_url = path(  g_tmp_dir ) / path(  g_valid_map_name );
           req->image_format = "png";
           req->map_mode = "trinary";
           req->free_thresh = g_default_free_thresh;
           req->occupied_thresh = g_default_occupied_thresh;
           auto resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   true );
          
           // 2. Load saved map and verify it
           nav_msgs::msg::OccupancyGrid map_msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
           verifyMapMsg(  map_msg );
          }
          
          // Send map saving service request with default parameters.
          // Load saved map and verify obtained OccupancyGrid.
          TEST_F(  MapSaverTestFixture,   SaveMapDefaultParameters )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing SaveMap service" );
           auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::SaveMap>(  
           "/map_saver/save_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for save_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           // 1. Send save_map serivce request with default parameters
           req->map_topic = "";
           req->map_url = path(  g_tmp_dir ) / path(  g_valid_map_name );
           req->image_format = "";
           req->map_mode = "";
           req->free_thresh = 0.0;
           req->occupied_thresh = 0.0;
           auto resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   true );
          
           // 2. Load saved map and verify it
           nav_msgs::msg::OccupancyGrid map_msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
           verifyMapMsg(  map_msg );
          }
          
          // Send map saving service requests with different sets of parameters.
          // In case of map is expected to be saved correctly,   load map from a saved
          // file and verify obtained OccupancyGrid.
          TEST_F(  MapSaverTestFixture,   SaveMapInvalidParameters )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing SaveMap service" );
           auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::SaveMap>(  
           "/map_saver/save_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for save_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           // 1. Trying to send save_map serivce request with different sets of parameters
           // In case of map is expected to be saved correctly,   verify it
           req->map_topic = "invalid_map";
           req->map_url = path(  g_tmp_dir ) / path(  g_valid_map_name );
           req->image_format = "png";
           req->map_mode = "trinary";
           req->free_thresh = g_default_free_thresh;
           req->occupied_thresh = g_default_occupied_thresh;
           auto resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   false );
          
           req->map_topic = "map";
           req->image_format = "invalid_format";
           resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   true );
           nav_msgs::msg::OccupancyGrid map_msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
           verifyMapMsg(  map_msg );
          
           req->image_format = "png";
           req->map_mode = "invalid_mode";
           resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   true );
           status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
           verifyMapMsg(  map_msg );
          
           req->map_mode = "trinary";
           req->free_thresh = 2.0;
           req->occupied_thresh = 2.0;
           resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   false );
          
           req->free_thresh = -2.0;
           req->occupied_thresh = -2.0;
           resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   false );
          
           req->free_thresh = 0.7;
           req->occupied_thresh = 0.2;
           resp = send_request<nav2_msgs::srv::SaveMap>(  node_,   client,   req );
           ASSERT_EQ(  resp->result,   false );
          }

navigation2/nav2_map_server/test/component/test_map_saver_publisher.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <experimental/filesystem>
          #include <string>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_map_server/map_io.hpp"
          #include "test_constants/test_constants.h"
          
          #define TEST_DIR TEST_DIRECTORY
          
          using namespace nav2_map_server; // NOLINT
          using std::experimental::filesystem::path;
          
      28  class TestPublisher : public rclcpp::Node
          {
          public:
      31   TestPublisher(   )
           : Node(  "map_publisher" )
           {
           std::string pub_map_file = path(  TEST_DIR ) / path(  g_valid_yaml_file );
           nav_msgs::msg::OccupancyGrid msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  pub_map_file,   msg );
           if (  status != LOAD_MAP_SUCCESS ) {
           RCLCPP_ERROR(  get_logger(   ),   "Can not load %s map file",   pub_map_file.c_str(   ) );
           return;
           }
          
           map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(  
           "map",  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
           map_pub_->publish(  msg );
           }
          
          protected:
      49   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
          };
          
      52  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto pub_node = std::make_shared<TestPublisher>(   );
           rclcpp::spin(  pub_node );
           rclcpp::shutdown(   );
           return 0;
          }

navigation2/nav2_map_server/test/component/test_map_server_node.cpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <string>
          #include <memory>
          #include <experimental/filesystem> // NOLINT
          
          #include <rclcpp/rclcpp.hpp>
          
          #include "test_constants/test_constants.h"
          #include "nav2_map_server/map_server.hpp"
          #include "nav2_util/lifecycle_service_client.hpp"
          #include "nav2_msgs/srv/load_map.hpp"
          using namespace std::chrono_literals;
          using namespace rclcpp; // NOLINT
          
          #define TEST_DIR TEST_DIRECTORY
          
          using std::experimental::filesystem::path;
          
          using lifecycle_msgs::msg::Transition;
          
      36  class RclCppFixture
          {
          public:
      39   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      40   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          
          RclCppFixture g_rclcppfixture;
          
      45  class MapServerTestFixture : public ::testing::Test
          {
          public:
      48   static void SetUpTestCase(   )
           {
           node_ = rclcpp::Node::make_shared(  "map_client_test" );
           lifecycle_client_ =
           std::make_shared<nav2_util::LifecycleServiceClient>(  "map_server",   node_ );
           RCLCPP_INFO(  node_->get_logger(   ),   "Creating Test Node" );
          
           std::this_thread::sleep_for(  std::chrono::seconds(  5 ) ); // allow node to start up
           const std::chrono::seconds timeout(  5 );
           lifecycle_client_->change_state(  Transition::TRANSITION_CONFIGURE,   timeout );
           lifecycle_client_->change_state(  Transition::TRANSITION_ACTIVATE,   timeout );
           }
          
      61   static void TearDownTestCase(   )
           {
           lifecycle_client_->change_state(  Transition::TRANSITION_DEACTIVATE );
           lifecycle_client_->change_state(  Transition::TRANSITION_CLEANUP );
           lifecycle_client_->change_state(  Transition::TRANSITION_UNCONFIGURED_SHUTDOWN );
           lifecycle_client_.reset(   );
           node_.reset(   );
           }
          
           template<class T>
           typename T::Response::SharedPtr send_request(  
           rclcpp::Node::SharedPtr node,  
           typename rclcpp::Client<T>::SharedPtr client,  
           typename T::Request::SharedPtr request )
           {
           auto result = client->async_send_request(  request );
          
           // Wait for the result
           if (  rclcpp::spin_until_future_complete(  node,   result ) == rclcpp::FutureReturnCode::SUCCESS ) {
           return result.get(   );
           } else {
           return nullptr;
           }
           }
          
          protected:
           // Check that map_msg corresponds to reference pattern
           // Input: map_msg
           void verifyMapMsg(  const nav_msgs::msg::OccupancyGrid & map_msg )
           {
           ASSERT_FLOAT_EQ(  map_msg.info.resolution,   g_valid_image_res );
           ASSERT_EQ(  map_msg.info.width,   g_valid_image_width );
           ASSERT_EQ(  map_msg.info.height,   g_valid_image_height );
           for (  unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
           ASSERT_EQ(  g_valid_image_content[i],   map_msg.data[i] );
           }
           }
          
           static rclcpp::Node::SharedPtr node_;
           static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
          };
          
          
          rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr;
          std::shared_ptr<nav2_util::LifecycleServiceClient> MapServerTestFixture::lifecycle_client_ =
           nullptr;
          
          
          // Send map getting service request and verify obtained OccupancyGrid
          TEST_F(  MapServerTestFixture,   GetMap )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing GetMap service" );
           auto req = std::make_shared<nav_msgs::srv::GetMap::Request>(   );
           auto client = node_->create_client<nav_msgs::srv::GetMap>(  
           "/map_server/map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           auto resp = send_request<nav_msgs::srv::GetMap>(  node_,   client,   req );
          
           verifyMapMsg(  resp->map );
          }
          
          // Send map loading service request and verify obtained OccupancyGrid
          TEST_F(  MapServerTestFixture,   LoadMap )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing LoadMap service" );
           auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::LoadMap>(  
           "/map_server/load_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for load_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           req->map_url = path(  TEST_DIR ) / path(  g_valid_yaml_file );
           auto resp = send_request<nav2_msgs::srv::LoadMap>(  node_,   client,   req );
          
           ASSERT_EQ(  resp->result,   nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS );
           verifyMapMsg(  resp->map );
          }
          
          // Send map loading service request without specifying which map to load
          TEST_F(  MapServerTestFixture,   LoadMapNull )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing LoadMap service" );
           auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::LoadMap>(  
           "/map_server/load_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for load_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           req->map_url = "";
           RCLCPP_INFO(  node_->get_logger(   ),   "Sending load_map request with null file name" );
           auto resp = send_request<nav2_msgs::srv::LoadMap>(  node_,   client,   req );
          
           ASSERT_EQ(  resp->result,   nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST );
          }
          
          // Send map loading service request with non-existing yaml file
          TEST_F(  MapServerTestFixture,   LoadMapInvalidYaml )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing LoadMap service" );
           auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::LoadMap>(  
           "/map_server/load_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for load_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           req->map_url = "invalid_file.yaml";
           RCLCPP_INFO(  node_->get_logger(   ),   "Sending load_map request with invalid yaml file name" );
           auto resp = send_request<nav2_msgs::srv::LoadMap>(  node_,   client,   req );
          
           ASSERT_EQ(  resp->result,   nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA );
          }
          
          // Send map loading service request with yaml file containing non-existing map
          TEST_F(  MapServerTestFixture,   LoadMapInvalidImage )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing LoadMap service" );
           auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>(   );
           auto client = node_->create_client<nav2_msgs::srv::LoadMap>(  
           "/map_server/load_map" );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for load_map service" );
           ASSERT_TRUE(  client->wait_for_service(   ) );
          
           req->map_url = path(  TEST_DIR ) / "invalid_image.yaml";
           RCLCPP_INFO(  node_->get_logger(   ),   "Sending load_map request with invalid image file name" );
           auto resp = send_request<nav2_msgs::srv::LoadMap>(  node_,   client,   req );
          
           ASSERT_EQ(  resp->result,   nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA );
          }
          
          /**
           * Test behaviour of server if yaml_filename is set to an empty string.
           */
          TEST_F(  MapServerTestFixture,   NoInitialMap )
          {
           // turn off node into unconfigured state
           lifecycle_client_->change_state(  Transition::TRANSITION_DEACTIVATE );
           lifecycle_client_->change_state(  Transition::TRANSITION_CLEANUP );
          
           auto client = node_->create_client<nav_msgs::srv::GetMap>(  "/map_server/map" );
           auto req = std::make_shared<nav_msgs::srv::GetMap::Request>(   );
          
           auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(  node_,   "map_server" );
           ASSERT_TRUE(  parameters_client->wait_for_service(  3s ) );
          
           // set yaml_filename-parameter to empty string (  essentially restart the node )
           RCLCPP_INFO(  node_->get_logger(   ),   "Removing yaml_filename-parameter before restarting" );
           parameters_client->set_parameters(  {Parameter(  "yaml_filename",   ParameterValue(  "" ) )} );
          
           // only configure node,   to test behaviour of service while node is not active
           lifecycle_client_->change_state(  Transition::TRANSITION_CONFIGURE,   3s );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Testing LoadMap service while not being active" );
           auto load_map_req = std::make_shared<nav2_msgs::srv::LoadMap::Request>(   );
           auto load_map_cl = node_->create_client<nav2_msgs::srv::LoadMap>(  "/map_server/load_map" );
          
           ASSERT_TRUE(  load_map_cl->wait_for_service(  3s ) );
           auto resp = send_request<nav2_msgs::srv::LoadMap>(  node_,   load_map_cl,   load_map_req );
          
           ASSERT_EQ(  resp->result,   nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE );
          
           // activate server and load map:
           lifecycle_client_->change_state(  Transition::TRANSITION_ACTIVATE,   3s );
           RCLCPP_INFO(  node_->get_logger(   ),   "active again" );
          
           load_map_req->map_url = path(  TEST_DIR ) / path(  g_valid_yaml_file );
           auto load_res = send_request<nav2_msgs::srv::LoadMap>(  node_,   load_map_cl,   load_map_req );
          
           ASSERT_EQ(  load_res->result,   nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS );
           verifyMapMsg(  load_res->map );
          }

navigation2/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp

       1  // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <experimental/filesystem>
          #include <string>
          #include <memory>
          #include <utility>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          
      24  TEST(  MapSaverCLI,   CLITest )
          {
           std::string path = "/tmp/";
           std::string file = "test_map";
           std::string file_path = path + file;
          
           rclcpp::init(  0,   nullptr );
          
           auto node = std::make_shared<rclcpp::Node>(  "CLI_Test_Node" );
           RCLCPP_INFO(  node->get_logger(   ),   "Testing Map Saver CLI" );
          
           auto publisher = node->create_publisher<nav_msgs::msg::OccupancyGrid>(  
           "/map",  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
          
           auto msg = std::make_unique<nav_msgs::msg::OccupancyGrid>(   );
           msg->header.frame_id = "map";
           msg->header.stamp = node->now(   );
           msg->info.map_load_time = node->now(   );
           msg->info.resolution = 0.05;
           msg->info.width = 3;
           msg->info.height = 3;
           msg->info.origin.position.x = 0.0;
           msg->info.origin.position.y = 0.0;
           msg->info.origin.orientation.w = 1.0;
           msg->data.resize(  9 );
           msg->data[0] = 0;
           msg->data[2] = 100;
           msg->data[1] = 101;
           msg->data[3] = 50;
          
           RCLCPP_INFO(  node->get_logger(   ),   "Publishing occupancy grid..." );
          
           publisher->publish(  std::move(  msg ) );
          
           rclcpp::Rate(  1 ).sleep(   );
          
           // succeed on real map
           RCLCPP_INFO(  node->get_logger(   ),   "Calling saver..." );
          
           EXPECT_FALSE(  std::experimental::filesystem::exists(  file_path + ".yaml" ) );
          
           std::string command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli -f " ) + file_path;
           auto return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   0 );
          
           rclcpp::Rate(  0.25 ).sleep(   );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Checking on file..." );
          
           EXPECT_TRUE(  std::experimental::filesystem::exists(  file_path + ".pgm" ) );
           EXPECT_EQ(  std::experimental::filesystem::file_size(  file_path + ".pgm" ),   20ul );
          
           if (  std::experimental::filesystem::exists(  file_path + ".yaml" ) ) {
           std::experimental::filesystem::remove(  file_path + ".yaml" );
           }
           if (  std::experimental::filesystem::exists(  file_path + ".pgm" ) ) {
           std::experimental::filesystem::remove(  file_path + ".pgm" );
           }
          
           // fail on bogus map
           RCLCPP_INFO(  node->get_logger(   ),   "Calling saver..." );
          
           EXPECT_FALSE(  std::experimental::filesystem::exists(  file_path + ".yaml" ) );
          
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli "
           "-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f " ) + file_path +
           std::string(  "--ros-args __node:=map_saver_test_node" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   65280 );
          
           rclcpp::Rate(  0.25 ).sleep(   );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Checking on file..." );
          
           EXPECT_FALSE(  std::experimental::filesystem::exists(  file_path + ".yaml" ) );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Testing help..." );
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli -h" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   0 );
          
           rclcpp::Rate(  0.5 ).sleep(   );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Testing invalid mode..." );
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli --mode fake_mode" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   0 );
          
           rclcpp::Rate(  0.5 ).sleep(   );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Testing missing argument..." );
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli --mode" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   65280 );
          
           rclcpp::Rate(  0.5 ).sleep(   );
          
           RCLCPP_INFO(  node->get_logger(   ),   "Testing wrong argument..." );
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli --free 0 0" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   65280 );
          
           rclcpp::Rate(  0.5 ).sleep(   );
          
           command =
           std::string(  
           "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node" );
           return_code = system(  command.c_str(   ) );
           EXPECT_EQ(  return_code,   0 );
          }

navigation2/nav2_map_server/test/test_constants/test_constants.h

       1  /*
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          #ifndef TEST_CONSTANTS__TEST_CONSTANTS_H_
          #define TEST_CONSTANTS__TEST_CONSTANTS_H_
          
          /* Author: Brian Gerkey */
          
          /* This file externs global constants shared among tests */
          
          #include <vector>
          
          extern const unsigned int g_valid_image_width;
          extern const unsigned int g_valid_image_height;
          extern const char g_valid_image_content[];
          extern const char * g_valid_map_name;
          extern const char * g_valid_png_file;
          extern const char * g_valid_bmp_file;
          extern const char * g_valid_pgm_file;
          extern const char * g_valid_yaml_file;
          extern const char * g_tmp_dir;
          
          extern const double g_valid_image_res;
          // *INDENT-OFF*
          // Uncrustify may incorrectly guide to add extra spaces in < double > during CI tests
          extern const std::vector<double> g_valid_origin;
          // *INDENT-ON*
          extern const double g_default_free_thresh;
          extern const double g_default_occupied_thresh;
          
          #endif // TEST_CONSTANTS__TEST_CONSTANTS_H_

navigation2/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <string>
          #include <memory>
          #include <chrono>
          #include <limits>
          #include <mutex>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "nav2_map_server/costmap_filter_info_server.hpp"
          
          using namespace std::chrono_literals;
          
          typedef std::recursive_mutex mutex_t;
          
          static const char FILTER_INFO_TOPIC[] = "filter_info";
          static const int TYPE = 1;
          static const char MASK_TOPIC[] = "mask";
          static const double BASE = 0.1;
          static const double MULTIPLIER = 0.2;
          
          static const double EPSILON = std::numeric_limits<float>::epsilon(   );
          
      39  class RclCppFixture
          {
          public:
      42   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      43   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      47  class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer
          {
          public:
      50   void start(   )
           {
           on_configure(  get_current_state(   ) );
           on_activate(  get_current_state(   ) );
           }
          
      56   void stop(   )
           {
           on_deactivate(  get_current_state(   ) );
           on_cleanup(  get_current_state(   ) );
           on_shutdown(  get_current_state(   ) );
           }
          };
          
      64  class InfoServerTester : public ::testing::Test
          {
          public:
      67   InfoServerTester(   )
           : info_server_(  nullptr ),   info_(  nullptr ),   subscription_(  nullptr )
           {
           access_ = new mutex_t(   );
          
           info_server_ = std::make_shared<InfoServerWrapper>(   );
           try {
           info_server_->set_parameter(  rclcpp::Parameter(  "filter_info_topic",   FILTER_INFO_TOPIC ) );
           info_server_->set_parameter(  rclcpp::Parameter(  "type",   TYPE ) );
           info_server_->set_parameter(  rclcpp::Parameter(  "mask_topic",   MASK_TOPIC ) );
           info_server_->set_parameter(  rclcpp::Parameter(  "base",   BASE ) );
           info_server_->set_parameter(  rclcpp::Parameter(  "multiplier",   MULTIPLIER ) );
           } catch (  rclcpp::exceptions::ParameterNotDeclaredException & ex ) {
           RCLCPP_ERROR(  
           info_server_->get_logger(   ),  
           "Error while setting parameters for CostmapFilterInfoServer: %s",   ex.what(   ) );
           throw;
           }
          
           info_server_->start(   );
          
           subscription_ = info_server_->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(  
           FILTER_INFO_TOPIC,   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &InfoServerTester::infoCallback,   this,   std::placeholders::_1 ) );
           }
          
      93   ~InfoServerTester(   )
           {
           info_server_->stop(   );
           info_server_.reset(   );
           subscription_.reset(   );
           }
          
     100   bool isReceived(   )
           {
           std::lock_guard<mutex_t> guard(  *getMutex(   ) );
           if (  info_ ) {
           return true;
           } else {
           return false;
           }
           }
          
     110   mutex_t * getMutex(   )
           {
           return access_;
           }
          
          protected:
     116   std::shared_ptr<InfoServerWrapper> info_server_;
     117   nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_;
          
          private:
     120   void infoCallback(  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
           {
           std::lock_guard<mutex_t> guard(  *getMutex(   ) );
           info_ = msg;
           }
          
     126   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr subscription_;
          
     128   mutex_t * access_;
          };
          
     131  TEST_F(  InfoServerTester,   testCostmapFilterInfoPublish )
          {
           rclcpp::Time start_time = info_server_->now(   );
           while (  !isReceived(   ) ) {
           rclcpp::spin_some(  info_server_->get_node_base_interface(   ) );
           std::this_thread::sleep_for(  100ms );
           // Waiting no more than 5 seconds
           ASSERT_TRUE(  (  info_server_->now(   ) - start_time ) <= rclcpp::Duration(  5000ms ) );
           }
          
           // Checking received CostmapFilterInfo for consistency
           EXPECT_EQ(  info_->type,   TYPE );
           EXPECT_EQ(  info_->filter_mask_topic,   MASK_TOPIC );
           EXPECT_NEAR(  info_->base,   BASE,   EPSILON );
           EXPECT_NEAR(  info_->multiplier,   MULTIPLIER,   EPSILON );
          }

navigation2/nav2_map_server/test/unit/test_map_io.cpp

       1  // Copyright 2019 Rover Robotics
          
          /*
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          /* Author: Brian Gerkey */
          
          #include <gtest/gtest.h>
          #include <experimental/filesystem>
          #include <stdexcept>
          #include <string>
          #include <vector>
          #include <memory>
          #include <iostream>
          #include <fstream>
          
          #include "yaml-cpp/yaml.h"
          #include "nav2_map_server/map_io.hpp"
          #include "nav2_map_server/map_server.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "test_constants/test_constants.h"
          
          #define TEST_DIR TEST_DIRECTORY
          
          using namespace std; // NOLINT
          using namespace nav2_map_server; // NOLINT
          using std::experimental::filesystem::path;
          
      55  class RclCppFixture
          {
          public:
      58   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      59   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          
          RclCppFixture g_rclcppfixture;
          
      64  class MapIOTester : public ::testing::Test
          {
          protected:
           // Fill LoadParameters with standard for testing values
           // Input: image_file_name
           // Output: load_parameters
      70   void fillLoadParameters(  
      71   const std::string & image_file_name,  
           LoadParameters & load_parameters )
           {
           load_parameters.image_file_name = image_file_name;
           load_parameters.resolution = g_valid_image_res;
           load_parameters.origin = g_valid_origin;
           load_parameters.free_thresh = g_default_free_thresh;
           load_parameters.occupied_thresh = g_default_occupied_thresh;
           load_parameters.mode = MapMode::Trinary;
           load_parameters.negate = 0;
           }
          
           // Fill SaveParameters with standard for testing values
           // Input: map_file_name,   image_format
           // Output: save_parameters
      86   void fillSaveParameters(  
      87   const std::string & map_file_name,  
      88   const std::string & image_format,  
      89   SaveParameters & save_parameters )
           {
           save_parameters.map_file_name = map_file_name;
           save_parameters.image_format = image_format;
           save_parameters.free_thresh = g_default_free_thresh;
           save_parameters.occupied_thresh = g_default_occupied_thresh;
           save_parameters.mode = MapMode::Trinary;
           }
          
           // Check that map_msg corresponds to reference pattern
           // Input: map_msg
     100   void verifyMapMsg(  const nav_msgs::msg::OccupancyGrid & map_msg )
           {
           ASSERT_FLOAT_EQ(  map_msg.info.resolution,   g_valid_image_res );
           ASSERT_EQ(  map_msg.info.width,   g_valid_image_width );
           ASSERT_EQ(  map_msg.info.height,   g_valid_image_height );
           for (  unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
           ASSERT_EQ(  g_valid_image_content[i],   map_msg.data[i] );
           }
           }
          };
          
          // Load a valid reference PGM file. Check obtained OccupancyGrid message for consistency:
          // loaded image should match the known dimensions and content of the file.
          // Save obtained OccupancyGrid message into a tmp PGM file. Then load back saved tmp file
          // and check for consistency.
          // Succeeds all steps were passed without a problem or expection.
     116  TEST_F(  MapIOTester,   loadSaveValidPGM )
          {
           // 1. Load reference map file and verify obtained OccupancyGrid
           LoadParameters loadParameters;
           fillLoadParameters(  path(  TEST_DIR ) / path(  g_valid_pgm_file ),   loadParameters );
          
           nav_msgs::msg::OccupancyGrid map_msg;
           ASSERT_NO_THROW(  loadMapFromFile(  loadParameters,   map_msg ) );
          
           verifyMapMsg(  map_msg );
          
           // 2. Save OccupancyGrid into a tmp file
           SaveParameters saveParameters;
           fillSaveParameters(  path(  g_tmp_dir ) / path(  g_valid_map_name ),   "pgm",   saveParameters );
          
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           // 3. Load saved map and verify it
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           verifyMapMsg(  map_msg );
          }
          
          // Load a valid reference PNG file. Check obtained OccupancyGrid message for consistency:
          // loaded image should match the known dimensions and content of the file.
          // Save obtained OccupancyGrid message into a tmp PNG file. Then load back saved tmp file
          // and check for consistency.
          // Succeeds all steps were passed without a problem or expection.
     145  TEST_F(  MapIOTester,   loadSaveValidPNG )
          {
           // 1. Load reference map file and verify obtained OccupancyGrid
           LoadParameters loadParameters;
           fillLoadParameters(  path(  TEST_DIR ) / path(  g_valid_png_file ),   loadParameters );
          
           nav_msgs::msg::OccupancyGrid map_msg;
           ASSERT_NO_THROW(  loadMapFromFile(  loadParameters,   map_msg ) );
          
           verifyMapMsg(  map_msg );
          
           // 2. Save OccupancyGrid into a tmp file
           SaveParameters saveParameters;
           fillSaveParameters(  path(  g_tmp_dir ) / path(  g_valid_map_name ),   "png",   saveParameters );
          
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           // 3. Load saved map and verify it
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           verifyMapMsg(  map_msg );
          }
          
          // Load a valid reference BMP file. Check obtained OccupancyGrid message for consistency:
          // loaded image should match the known dimensions and content of the file.
          // Save obtained OccupancyGrid message into a tmp BMP file. Then load back saved tmp file
          // and check for consistency.
          // Succeeds all steps were passed without a problem or expection.
     174  TEST_F(  MapIOTester,   loadSaveValidBMP )
          {
           // 1. Load reference map file and verify obtained OccupancyGrid
           auto test_bmp = path(  TEST_DIR ) / path(  g_valid_bmp_file );
          
           LoadParameters loadParameters;
           fillLoadParameters(  test_bmp,   loadParameters );
          
           nav_msgs::msg::OccupancyGrid map_msg;
           ASSERT_NO_THROW(  loadMapFromFile(  loadParameters,   map_msg ) );
          
           verifyMapMsg(  map_msg );
          
           // 2. Save OccupancyGrid into a tmp file
           SaveParameters saveParameters;
           fillSaveParameters(  path(  g_tmp_dir ) / path(  g_valid_map_name ),   "bmp",   saveParameters );
          
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           // 3. Load saved map and verify it
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           verifyMapMsg(  map_msg );
          }
          
          // Load map from a valid file. Trying to save map with different modes.
          // Succeeds all steps were passed without a problem or expection.
     202  TEST_F(  MapIOTester,   loadSaveMapModes )
          {
           // 1. Load map from YAML file
           nav_msgs::msg::OccupancyGrid map_msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  TEST_DIR ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           // No need to check Trinary mode. This already verified in previous testcases.
           // 2. Save map in Scale mode.
           SaveParameters saveParameters;
           fillSaveParameters(  path(  g_tmp_dir ) / path(  g_valid_map_name ),   "png",   saveParameters );
           saveParameters.mode = MapMode::Scale;
          
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           // 3. Load saved map and verify it
           status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           verifyMapMsg(  map_msg );
          
           // 4. Save map in Raw mode.
           saveParameters.mode = MapMode::Raw;
          
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           // 5. Load saved map and verify it
           status = loadMapFromYaml(  path(  g_tmp_dir ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           verifyMapMsg(  map_msg );
          }
          
          // Try to load an invalid file with different ways.
          // Succeeds if all cases are got expected fail behaviours.
     237  TEST_F(  MapIOTester,   loadInvalidFile )
          {
           // 1. Trying to load incorrect map by loadMapFromFile(   )
           auto test_invalid = path(  TEST_DIR ) / path(  "foo" );
          
           LoadParameters loadParameters;
           fillLoadParameters(  test_invalid,   loadParameters );
          
           nav_msgs::msg::OccupancyGrid map_msg;
           ASSERT_ANY_THROW(  loadMapFromFile(  loadParameters,   map_msg ) );
          
           // 2. Trying to load incorrect map by loadMapFromYaml(   )
           LOAD_MAP_STATUS status = loadMapFromYaml(  "",   map_msg );
           ASSERT_EQ(  status,   MAP_DOES_NOT_EXIST );
          
           status = loadMapFromYaml(  std::string(  test_invalid ) + ".yaml",   map_msg );
           ASSERT_EQ(  status,   INVALID_MAP_METADATA );
          }
          
          // Load map from a valid file. Trying to save map with different sets of parameters.
          // Succeeds if all cases got expected behaviours.
     258  TEST_F(  MapIOTester,   saveInvalidParameters )
          {
           // 1. Load map from YAML file
           nav_msgs::msg::OccupancyGrid map_msg;
           LOAD_MAP_STATUS status = loadMapFromYaml(  path(  TEST_DIR ) / path(  g_valid_yaml_file ),   map_msg );
           ASSERT_EQ(  status,   LOAD_MAP_SUCCESS );
          
           // 2. Trying to save map with different sets of parameters
           SaveParameters saveParameters;
          
           saveParameters.map_file_name = path(  g_tmp_dir ) / path(  g_valid_map_name );
           saveParameters.image_format = "";
           saveParameters.free_thresh = 2.0;
           saveParameters.occupied_thresh = 2.0;
           saveParameters.mode = MapMode::Trinary;
           ASSERT_FALSE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           saveParameters.free_thresh = -2.0;
           saveParameters.occupied_thresh = -2.0;
           ASSERT_FALSE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           saveParameters.free_thresh = 0.7;
           saveParameters.occupied_thresh = 0.2;
           ASSERT_FALSE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           saveParameters.free_thresh = 0.0;
           saveParameters.occupied_thresh = 0.0;
           ASSERT_TRUE(  saveMapToFile(  map_msg,   saveParameters ) );
          
           saveParameters.map_file_name = path(  "/invalid_path" ) / path(  g_valid_map_name );
           ASSERT_FALSE(  saveMapToFile(  map_msg,   saveParameters ) );
          }
          
          // Load valid YAML file and check for consistency
     292  TEST_F(  MapIOTester,   loadValidYAML )
          {
           LoadParameters loadParameters;
           ASSERT_NO_THROW(  loadParameters = loadMapYaml(  path(  TEST_DIR ) / path(  g_valid_yaml_file ) ) );
          
           LoadParameters refLoadParameters;
           fillLoadParameters(  path(  TEST_DIR ) / path(  g_valid_png_file ),   refLoadParameters );
           ASSERT_EQ(  loadParameters.image_file_name,   refLoadParameters.image_file_name );
           ASSERT_FLOAT_EQ(  loadParameters.resolution,   refLoadParameters.resolution );
           ASSERT_EQ(  loadParameters.origin,   refLoadParameters.origin );
           ASSERT_FLOAT_EQ(  loadParameters.free_thresh,   refLoadParameters.free_thresh );
           ASSERT_FLOAT_EQ(  loadParameters.occupied_thresh,   refLoadParameters.occupied_thresh );
           ASSERT_EQ(  loadParameters.mode,   refLoadParameters.mode );
           ASSERT_EQ(  loadParameters.negate,   refLoadParameters.negate );
          }
          
          // Try to load invalid YAML file
     309  TEST_F(  MapIOTester,   loadInvalidYAML )
          {
           LoadParameters loadParameters;
           ASSERT_ANY_THROW(  loadParameters = loadMapYaml(  path(  TEST_DIR ) / path(  "invalid_file.yaml" ) ) );
          }

navigation2/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

       1  // Copyright (  c ) 2008,   Willow Garage,   Inc.
          // All rights reserved.
          //
          // Software License Agreement (  BSD License 2.0 )
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions
          // are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          // * Redistributions in binary form must reproduce the above
          // copyright notice,   this list of conditions and the following
          // disclaimer in the documentation and/or other materials provided
          // with the distribution.
          // * Neither the name of the Willow Garage nor the names of its
          // contributors may be used to endorse or promote products derived
          // from this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          // LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          // INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          // BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          // LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          // CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          // LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          // ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          //
          // Navigation function computation
          // Uses Dijkstra's method
          // Modified for Euclidean-distance computation
          //
          
          #ifndef NAV2_NAVFN_PLANNER__NAVFN_HPP_
          #define NAV2_NAVFN_PLANNER__NAVFN_HPP_
          
          #include <math.h>
          #include <stdint.h>
          #include <string.h>
          #include <stdio.h>
          
          namespace nav2_navfn_planner
          {
          
          // cost defs
          #define COST_UNKNOWN_ROS 255 // 255 is unknown cost
          #define COST_OBS 254 // 254 for forbidden regions
          #define COST_OBS_ROS 253 // ROS values of 253 are obstacles
          
          // navfn cost values are set to
          // COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
          // Incoming costmap cost values are in the range 0 to 252.
          // With COST_NEUTRAL of 50,   the COST_FACTOR needs to be about 0.8 to
          // ensure the input values are spread evenly over the output range,   50
          // to 253. If COST_FACTOR is higher,   cost values will have a plateau
          // around obstacles and the planner will then treat (  for example ) the
          // whole width of a narrow hallway as equally undesirable and thus
          // will not plan paths down the center.
          
          #define COST_NEUTRAL 50 // Set this to "open space" value
          #define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap(   )
          
          // Define the cost type in the case that it is not set. However,   this allows
          // clients to modify it without changing the file. Arguably,   it is better to require it to
          // be defined by a user explicitly
          #ifndef COSTTYPE
          #define COSTTYPE unsigned char // Whatever is used...
          #endif
          
          // potential defs
          #define POT_HIGH 1.0e10 // unassigned cell potential
          
          // priority buffers
          #define PRIORITYBUFSIZE 10000
          
          /**
           Navigation function call.
           \param costmap Cost map array,   of type COSTTYPE; origin is upper left
           NOTE: will be modified to have a border of obstacle costs
           \param nx Width of map in cells
           \param ny Height of map in cells
           \param goal X,  Y position of goal cell
           \param start X,  Y position of start cell
          
          Returns length of plan if found,   and fills an array with x,  y interpolated
          positions at about 1/2 cell resolution; else returns 0.
          */
      93  int create_nav_plan_astar(  
           const COSTTYPE * costmap,   int nx,   int ny,  
           int * goal,   int * start,  
           float * plan,   int nplan );
          
          /**
           * @class NavFn
           * @brief Navigation function class. Holds buffers for costmap,   navfn map. Maps are pixel-based.
           * Origin is upper left,   x is right,   y is down.
           */
     103  class NavFn
          {
          public:
           /**
           * @brief Constructs the planner
           * @param nx The x size of the map
           * @param ny The y size of the map
           */
     111   NavFn(  int nx,   int ny );
          
     113   ~NavFn(   );
          
           /**
           * @brief Sets or resets the size of the map
           * @param nx The x size of the map
           * @param ny The y size of the map
           */
     120   void setNavArr(  int nx,   int ny );
           int nx,   ny,   ns; /**< size of grid,   in pixels */
          
           /**
           * @brief Set up the cost array for the planner,   usually from ROS
           * @param cmap The costmap
           * @param isROS Whether or not the costmap is coming in in ROS format
           * @param allow_unknown Whether or not the planner should be allowed to plan through
           * unknown space
           */
     130   void setCostmap(  const COSTTYPE * cmap,   bool isROS = true,   bool allow_unknown = true );
          
           /**
           * @brief Calculates a plan using the A* heuristic,   returns true if one is found
           * @return True if a plan is found,   false otherwise
           */
     136   bool calcNavFnAstar(   );
          
           /**
           * @brief Caclulates the full navigation function using Dijkstra
           */
     141   bool calcNavFnDijkstra(  bool atStart = false );
          
           /**
           * @brief Accessor for the x-coordinates of a path
           * @return The x-coordinates of a path
           */
     147   float * getPathX(   );
          
           /**
           * @brief Accessor for the y-coordinates of a path
           * @return The y-coordinates of a path
           */
     153   float * getPathY(   );
          
           /**
           * @brief Accessor for the length of a path
           * @return The length of a path,   0 if not found
           */
     159   int getPathLen(   );
          
           /**
           * @brief Gets the cost of the path found the last time a navigation function was computed
           * @return The cost of the last path found
           */
     165   float getLastPathCost(   );
          
           /** cell arrays */
     168   COSTTYPE * costarr; /**< cost array in 2D configuration space */
           float * potarr; /**< potential array,   navigation function potential */
     170   bool * pending; /**< pending cells during propagation */
           int nobs; /**< number of obstacle cells */
          
           /** block priority buffers */
           int * pb1,   * pb2,   * pb3; /**< storage buffers for priority blocks */
           int * curP,   * nextP,   * overP; /**< priority buffer block ptrs */
           int curPe,   nextPe,   overPe; /**< end points of arrays */
          
           /** block priority thresholds */
           float curT; /**< current threshold */
           float priInc; /**< priority threshold increment */
          
           /** goal and start positions */
           /**
           * @brief Sets the goal position for the planner.
           * Note: the navigation cost field computed gives the cost to get to a given point
           * from the goal,   not from the start.
           * @param goal the goal position
           */
     189   void setGoal(  int * goal );
          
           /**
           * @brief Sets the start position for the planner.
           * Note: the navigation cost field computed gives the cost to get to a given point
           * from the goal,   not from the start.
           * @param start the start position
           */
     197   void setStart(  int * start );
          
           int goal[2];
           int start[2];
           /**
           * @brief Initialize cell k with cost v for propagation
           * @param k the cell to initialize
           * @param v the cost to give to the cell
           */
     206   void initCost(  int k,   float v );
          
           /** propagation */
          
           /**
           * @brief Updates the cell at index n
           * @param n The index to update
           */
     214   void updateCell(  int n );
          
           /**
           * @brief Updates the cell at index n using the A* heuristic
           * @param n The index to update
           */
     220   void updateCellAstar(  int n );
          
           /**
           * @brief Set up navigation potential arrays for new propagation
           * @param keepit whether or not use COST_NEUTRAL
           */
     226   void setupNavFn(  bool keepit = false );
          
           /**
           * @brief Run propagation for <cycles> iterations,   or until start is reached using
           * breadth-first Dijkstra method
           * @param cycles The maximum number of iterations to run for
           * @param atStart Whether or not to stop when the start point is reached
           * @return true if the start point is reached
           */
     235   bool propNavFnDijkstra(  int cycles,   bool atStart = false );
          
           /**
           * @brief Run propagation for <cycles> iterations,   or until start is reached using
           * the best-first A* method with Euclidean distance heuristic
           * @param cycles The maximum number of iterations to run for
           * @return true if the start point is reached
           */
     243   bool propNavFnAstar(  int cycles ); /**< returns true if start point found */
          
           /** gradient and paths */
           float * gradx,   * grady; /**< gradient arrays,   size of potential array */
           float * pathx,   * pathy; /**< path points,   as subpixel cell coordinates */
           int npath; /**< number of path points */
           int npathbuf; /**< size of pathx,   pathy buffers */
          
           float last_path_cost_; /**< Holds the cost of the path found the last time A* was called */
          
           /**
           * @brief Calculates the path for at mose <n> cycles
           * @param n The maximum number of cycles to run for
           * @return The lenght of the path found,   0 if none
           */
     258   int calcPath(  int n,   int * st = NULL );
          
           /**
           * @brief Calculate gradient at a cell
           * @param n Cell number <n>
           * @return float norm
           */
     265   float gradCell(  int n ); /**< calculates gradient at cell <n>,   returns norm */
          
           float pathStep; /**< step size for following gradient */
          
           /** display callback */
           /**< <n> is the number of cycles between updates */
           // void display(  void fn(  NavFn * nav ),   int n = 100 );
           // int displayInt; /**< save second argument of display(   ) above */
           // void (  * displayFn )(  NavFn * nav ); /**< display function itself */
          
           /** save costmap */
           /**< write out costmap and start/goal states as fname.pgm and fname.txt */
           // void savemap(  const char * fname );
          };
          
          } // namespace nav2_navfn_planner
          
          #endif // NAV2_NAVFN_PLANNER__NAVFN_HPP_

navigation2/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2018 Simbe Robotics
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
          #define NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
          
          #include <chrono>
          #include <string>
          #include <memory>
          #include <vector>
          
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_navfn_planner/navfn.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          namespace nav2_navfn_planner
          {
          
      38  class NavfnPlanner : public nav2_core::GlobalPlanner
          {
          public:
           /**
           * @brief constructor
           */
      44   NavfnPlanner(   );
          
           /**
           * @brief destructor
           */
      49   ~NavfnPlanner(   );
          
           /**
           * @brief Configuring plugin
           * @param parent Lifecycle node pointer
           * @param name Name of plugin map
           * @param tf Shared ptr of TF2 buffer
           * @param costmap_ros Costmap2DROS object
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup lifecycle node
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate lifecycle node
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate lifecycle node
           */
           void deactivate(   ) override;
          
          
           /**
           * @brief Creating a plan from start and goal poses
           * @param start Start pose
           * @param goal Goal pose
           * @return nav_msgs::Path of the generated path
           */
           nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) override;
          
          protected:
           /**
           * @brief Compute a plan given start and goal poses,   provided in global world frame.
           * @param start Start pose
           * @param goal Goal pose
           * @param tolerance Relaxation constraint in x and y
           * @param plan Path to be computed
           * @return true if can find the path
           */
           bool makePlan(  
           const geometry_msgs::msg::Pose & start,  
           const geometry_msgs::msg::Pose & goal,   double tolerance,  
           nav_msgs::msg::Path & plan );
          
           /**
           * @brief Compute the navigation function given a seed point in the world to start from
           * @param world_point Point in world coordinate frame
           * @return true if can compute
           */
           bool computePotential(  const geometry_msgs::msg::Point & world_point );
          
           /**
           * @brief Compute a plan to a goal from a potential - must call computePotential first
           * @param goal Goal pose
           * @param plan Path to be computed
           * @return true if can compute a plan path
           */
           bool getPlanFromPotential(  
           const geometry_msgs::msg::Pose & goal,  
           nav_msgs::msg::Path & plan );
          
           /**
           * @brief Remove artifacts at the end of the path - originated from planning on a discretized world
           * @param goal Goal pose
           * @param plan Computed path
           */
           void smoothApproachToGoal(  
           const geometry_msgs::msg::Pose & goal,  
           nav_msgs::msg::Path & plan );
          
           /**
           * @brief Compute the potential,   or navigation cost,   at a given point in the world
           * must call computePotential first
           * @param world_point Point in world coordinate frame
           * @return double point potential (  navigation cost )
           */
           double getPointPotential(  const geometry_msgs::msg::Point & world_point );
          
           // Check for a valid potential value at a given point in the world
           // - must call computePotential first
           // - currently unused
           // bool validPointPotential(  const geometry_msgs::msg::Point & world_point );
           // bool validPointPotential(  const geometry_msgs::msg::Point & world_point,   double tolerance );
          
           /**
           * @brief Compute the squared distance between two points
           * @param p1 Point 1
           * @param p2 Point 2
           * @return double squared distance between two points
           */
           inline double squared_distance(  
           const geometry_msgs::msg::Pose & p1,  
           const geometry_msgs::msg::Pose & p2 )
           {
           double dx = p1.position.x - p2.position.x;
           double dy = p1.position.y - p2.position.y;
           return dx * dx + dy * dy;
           }
          
           /**
           * @brief Transform a point from world to map frame
           * @param wx double of world X coordinate
           * @param wy double of world Y coordinate
           * @param mx int of map X coordinate
           * @param my int of map Y coordinate
           * @return true if can transform
           */
           bool worldToMap(  double wx,   double wy,   unsigned int & mx,   unsigned int & my );
          
           /**
           * @brief Transform a point from map to world frame
           * @param mx double of map X coordinate
           * @param my double of map Y coordinate
           * @param wx double of world X coordinate
           * @param wy double of world Y coordinate
           */
     175   void mapToWorld(  double mx,   double my,   double & wx,   double & wy );
          
           /**
           * @brief Set the corresponding cell cost to be free space
           * @param mx int of map X coordinate
           * @param my int of map Y coordinate
           */
     182   void clearRobotCell(  unsigned int mx,   unsigned int my );
          
           /**
           * @brief Determine if a new planner object should be made
           * @return true if planner object is out of date
           */
     188   bool isPlannerOutOfDate(   );
          
           // Planner based on ROS1 NavFn algorithm
           std::unique_ptr<NavFn> planner_;
          
           // TF buffer
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           // Clock
           rclcpp::Clock::SharedPtr clock_;
          
           // Logger
     200   rclcpp::Logger logger_{rclcpp::get_logger(  "NavfnPlanner" )};
          
           // Global Costmap
           nav2_costmap_2d::Costmap2D * costmap_;
          
           // The global frame of the costmap
           std::string global_frame_,   name_;
          
           // Whether or not the planner should be allowed to plan through unknown space
           bool allow_unknown_,   use_final_approach_orientation_;
          
           // If the goal is obstructed,   the tolerance specifies how many meters the planner
           // can relax the constraint in x and y before failing
           double tolerance_;
          
           // Whether to use the astar planner or default dijkstras
           bool use_astar_;
          
           // parent node weak ptr
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
     229   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          
          } // namespace nav2_navfn_planner
          
          #endif // NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_

navigation2/nav2_navfn_planner/src/navfn.cpp

       1  // Copyright (  c ) 2008,   Willow Garage,   Inc.
          // All rights reserved.
          //
          // Software License Agreement (  BSD License 2.0 )
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions
          // are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          // * Redistributions in binary form must reproduce the above
          // copyright notice,   this list of conditions and the following
          // disclaimer in the documentation and/or other materials provided
          // with the distribution.
          // * Neither the name of the Willow Garage nor the names of its
          // contributors may be used to endorse or promote products derived
          // from this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          // LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          // INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          // BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          // LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          // CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          // LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          // ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          //
          // Navigation function computation
          // Uses Dijkstra's method
          // Modified for Euclidean-distance computation
          //
          // Path calculation uses no interpolation when pot field is at max in
          // nearby cells
          //
          // Path calc has sanity check that it succeeded
          //
          
          #include "nav2_navfn_planner/navfn.hpp"
          
          #include <algorithm>
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_navfn_planner
          {
          
          //
          // function to perform nav fn calculation
          // keeps track of internal buffers,   will be more efficient
          // if the size of the environment does not change
          //
          
          // Example usage:
          /*
          int
          create_nav_plan_astar(  
           COSTTYPE * costmap,   int nx,   int ny,  
           int * goal,   int * start,  
           float * plan,   int nplan )
          {
           static NavFn * nav = NULL;
          
           if (  nav == NULL ) {
           nav = new NavFn(  nx,   ny );
           }
          
           if (  nav->nx != nx || nav->ny != ny ) { // check for compatibility with previous call
           delete nav;
           nav = new NavFn(  nx,   ny );
           }
          
           nav->setGoal(  goal );
           nav->setStart(  start );
          
           nav->costarr = costmap;
           nav->setupNavFn(  true );
          
           // calculate the nav fn and path
           nav->priInc = 2 * COST_NEUTRAL;
           nav->propNavFnAstar(  std::max(  nx * ny / 20,   nx + ny ) );
          
           // path
           int len = nav->calcPath(  nplan );
          
           if (  len > 0 ) { // found plan
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[NavFn] Path found,   %d steps\n",   len );
           } else {
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[NavFn] No path found\n" );
           }
          
           if (  len > 0 ) {
           for (  int i = 0; i < len; i++ ) {
           plan[i * 2] = nav->pathx[i];
           plan[i * 2 + 1] = nav->pathy[i];
           }
           }
          
           return len;
          }
          */
          
          //
          // create nav fn buffers
          //
          
     111  NavFn::NavFn(  int xs,   int ys )
          {
           // create cell arrays
           costarr = NULL;
           potarr = NULL;
           pending = NULL;
           gradx = grady = NULL;
           setNavArr(  xs,   ys );
          
           // priority buffers
           pb1 = new int[PRIORITYBUFSIZE];
           pb2 = new int[PRIORITYBUFSIZE];
           pb3 = new int[PRIORITYBUFSIZE];
          
           // for Dijkstra (  breadth-first ),   set to COST_NEUTRAL
           // for A* (  best-first ),   set to COST_NEUTRAL
           priInc = 2 * COST_NEUTRAL;
          
           // goal and start
           goal[0] = goal[1] = 0;
           start[0] = start[1] = 0;
          
           // display function
           // displayFn = NULL;
           // displayInt = 0;
          
           // path buffers
           npathbuf = npath = 0;
           pathx = pathy = NULL;
           pathStep = 0.5;
          }
          
          
     144  NavFn::~NavFn(   )
          {
           if (  costarr ) {
           delete[] costarr;
           }
           if (  potarr ) {
           delete[] potarr;
           }
           if (  pending ) {
           delete[] pending;
           }
           if (  gradx ) {
           delete[] gradx;
           }
           if (  grady ) {
           delete[] grady;
           }
           if (  pathx ) {
           delete[] pathx;
           }
           if (  pathy ) {
           delete[] pathy;
           }
           if (  pb1 ) {
           delete[] pb1;
           }
           if (  pb2 ) {
           delete[] pb2;
           }
           if (  pb3 ) {
           delete[] pb3;
           }
          }
          
          
          //
          // set goal,   start positions for the nav fn
          //
          
          void
     184  NavFn::setGoal(  int * g )
          {
           goal[0] = g[0];
           goal[1] = g[1];
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[NavFn] Setting goal to %d,  %d\n",   goal[0],   goal[1] );
          }
          
          void
     192  NavFn::setStart(  int * g )
          {
           start[0] = g[0];
           start[1] = g[1];
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),   "[NavFn] Setting start to %d,  %d\n",   start[0],  
           start[1] );
          }
          
          //
          // Set/Reset map size
          //
          
          void
     206  NavFn::setNavArr(  int xs,   int ys )
          {
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[NavFn] Array is %d x %d\n",   xs,   ys );
          
           nx = xs;
           ny = ys;
           ns = nx * ny;
          
           if (  costarr ) {
           delete[] costarr;
           }
           if (  potarr ) {
           delete[] potarr;
           }
           if (  pending ) {
           delete[] pending;
           }
          
           if (  gradx ) {
           delete[] gradx;
           }
           if (  grady ) {
           delete[] grady;
           }
          
           costarr = new COSTTYPE[ns]; // cost array,   2d config space
           memset(  costarr,   0,   ns * sizeof(  COSTTYPE ) );
           potarr = new float[ns]; // navigation potential array
           pending = new bool[ns];
           memset(  pending,   0,   ns * sizeof(  bool ) );
           gradx = new float[ns];
           grady = new float[ns];
          }
          
          
          //
          // set up cost array,   usually from ROS
          //
          
          void
     246  NavFn::setCostmap(  const COSTTYPE * cmap,   bool isROS,   bool allow_unknown )
          {
           COSTTYPE * cm = costarr;
           if (  isROS ) { // ROS-type cost array
           for (  int i = 0; i < ny; i++ ) {
           int k = i * nx;
           for (  int j = 0; j < nx; j++,   k++,   cmap++,   cm++ ) {
           // This transforms the incoming cost values:
           // COST_OBS -> COST_OBS (  incoming "lethal obstacle" )
           // COST_OBS_ROS -> COST_OBS (  incoming "inscribed inflated obstacle" )
           // values in range 0 to 252 -> values from COST_NEUTRAL to COST_OBS_ROS.
           *cm = COST_OBS;
           int v = *cmap;
           if (  v < COST_OBS_ROS ) {
           v = COST_NEUTRAL + COST_FACTOR * v;
           if (  v >= COST_OBS ) {
           v = COST_OBS - 1;
           }
           *cm = v;
           } else if (  v == COST_UNKNOWN_ROS && allow_unknown ) {
           v = COST_OBS - 1;
           *cm = v;
           }
           }
           }
           } else { // not a ROS map,   just a PGM
           for (  int i = 0; i < ny; i++ ) {
           int k = i * nx;
           for (  int j = 0; j < nx; j++,   k++,   cmap++,   cm++ ) {
           *cm = COST_OBS;
           if (  i < 7 || i > ny - 8 || j < 7 || j > nx - 8 ) {
           continue; // don't do borders
           }
           int v = *cmap;
           if (  v < COST_OBS_ROS ) {
           v = COST_NEUTRAL + COST_FACTOR * v;
           if (  v >= COST_OBS ) {
           v = COST_OBS - 1;
           }
           *cm = v;
           } else if (  v == COST_UNKNOWN_ROS ) {
           v = COST_OBS - 1;
           *cm = v;
           }
           }
           }
           }
          }
          
          bool
     296  NavFn::calcNavFnDijkstra(  bool atStart )
          {
           setupNavFn(  true );
          
           // calculate the nav fn and path
           return propNavFnDijkstra(  std::max(  nx * ny / 20,   nx + ny ),   atStart );
          }
          
          
          //
          // calculate navigation function,   given a costmap,   goal,   and start
          //
          
          bool
     310  NavFn::calcNavFnAstar(   )
          {
           setupNavFn(  true );
          
           // calculate the nav fn and path
           return propNavFnAstar(  std::max(  nx * ny / 20,   nx + ny ) );
          }
          
          //
          // returning values
          //
          
     322  float * NavFn::getPathX(   ) {return pathx;}
     323  float * NavFn::getPathY(   ) {return pathy;}
     324  int NavFn::getPathLen(   ) {return npath;}
          
          // inserting onto the priority blocks
          #define push_cur(  n ) {if (  n >= 0 && n < ns && !pending[n] && \
           costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE ) \
           {curP[curPe++] = n; pending[n] = true;}}
          #define push_next(  n ) {if (  n >= 0 && n < ns && !pending[n] && \
           costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE ) \
           {nextP[nextPe++] = n; pending[n] = true;}}
          #define push_over(  n ) {if (  n >= 0 && n < ns && !pending[n] && \
           costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE ) \
     335   {overP[overPe++] = n; pending[n] = true;}}
          
          
          // Set up navigation potential arrays for new propagation
          
          void
          NavFn::setupNavFn(  bool keepit )
          {
           // reset values in propagation arrays
           for (  int i = 0; i < ns; i++ ) {
           potarr[i] = POT_HIGH;
           if (  !keepit ) {
           costarr[i] = COST_NEUTRAL;
           }
           gradx[i] = grady[i] = 0.0;
           }
          
           // outer bounds of cost array
           COSTTYPE * pc;
           pc = costarr;
           for (  int i = 0; i < nx; i++ ) {
           *pc++ = COST_OBS;
           }
           pc = costarr + (  ny - 1 ) * nx;
           for (  int i = 0; i < nx; i++ ) {
           *pc++ = COST_OBS;
           }
           pc = costarr;
           for (  int i = 0; i < ny; i++,   pc += nx ) {
           *pc = COST_OBS;
           }
           pc = costarr + nx - 1;
           for (  int i = 0; i < ny; i++,   pc += nx ) {
           *pc = COST_OBS;
           }
          
           // priority buffers
           curT = COST_OBS;
           curP = pb1;
           curPe = 0;
           nextP = pb2;
           nextPe = 0;
           overP = pb3;
           overPe = 0;
           memset(  pending,   0,   ns * sizeof(  bool ) );
          
           // set goal
           int k = goal[0] + goal[1] * nx;
           initCost(  k,   0 );
          
           // find # of obstacle cells
           pc = costarr;
           int ntot = 0;
           for (  int i = 0; i < ns; i++,   pc++ ) {
           if (  *pc >= COST_OBS ) {
           ntot++; // number of cells that are obstacles
           }
           }
           nobs = ntot;
     394  }
          
          
          // initialize a goal-type cost for starting propagation
          
          void
          NavFn::initCost(  int k,   float v )
          {
           potarr[k] = v;
           push_cur(  k + 1 );
           push_cur(  k - 1 );
           push_cur(  k - nx );
           push_cur(  k + nx );
          }
          
          
          //
          // Critical function: calculate updated potential value of a cell,  
          // given its neighbors' values
          // Planar-wave update calculation from two lowest neighbors in a 4-grid
          // Quadratic approximation to the interpolated value
     415  // No checking of bounds here,   this function should be fast
          //
          
          #define INVSQRT2 0.707106781
          
          inline void
          NavFn::updateCell(  int n )
          {
           // get neighbors
           float u,   d,   l,   r;
           l = potarr[n - 1];
           r = potarr[n + 1];
           u = potarr[n - nx];
           d = potarr[n + nx];
           // ROS_INFO(  "[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",  
           // potarr[n],   l,   r,   u,   d );
           // ROS_INFO(  "[Update] cost: %d\n",   costarr[n] );
          
           // find lowest,   and its lowest neighbor
           float ta,   tc;
           if (  l < r ) {tc = l;} else {tc = r;}
           if (  u < d ) {ta = u;} else {ta = d;}
          
           // do planar wave update
           if (  costarr[n] < COST_OBS ) { // don't propagate into obstacles
           float hf = static_cast<float>(  costarr[n] ); // traversability factor
           float dc = tc - ta; // relative cost between ta,  tc
           if (  dc < 0 ) { // ta is lowest
           dc = -dc;
           ta = tc;
           }
          
           // calculate new potential
           float pot;
           if (  dc >= hf ) { // if too large,   use ta-only update
           pot = ta + hf;
           } else { // two-neighbor interpolation update
           // use quadratic approximation
           // might speed this up through table lookup,   but still have to
           // do the divide
           float d = dc / hf;
           float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
           pot = ta + hf * v;
           }
          
           // ROS_INFO(  "[Update] new pot: %d\n",   costarr[n] );
          
           // now add affected neighbors to priority blocks
           if (  pot < potarr[n] ) {
           float le = INVSQRT2 * static_cast<float>(  costarr[n - 1] );
           float re = INVSQRT2 * static_cast<float>(  costarr[n + 1] );
           float ue = INVSQRT2 * static_cast<float>(  costarr[n - nx] );
           float de = INVSQRT2 * static_cast<float>(  costarr[n + nx] );
           potarr[n] = pot;
           if (  pot < curT ) { // low-cost buffer block
           if (  l > pot + le ) {push_next(  n - 1 );}
           if (  r > pot + re ) {push_next(  n + 1 );}
           if (  u > pot + ue ) {push_next(  n - nx );}
           if (  d > pot + de ) {push_next(  n + nx );}
           } else { // overflow block
           if (  l > pot + le ) {push_over(  n - 1 );}
           if (  r > pot + re ) {push_over(  n + 1 );}
           if (  u > pot + ue ) {push_over(  n - nx );}
           if (  d > pot + de ) {push_over(  n + nx );}
           }
           }
           }
          }
          
          //
          // Use A* method for setting priorities
          // Critical function: calculate updated potential value of a cell,  
          // given its neighbors' values
          // Planar-wave update calculation from two lowest neighbors in a 4-grid
          // Quadratic approximation to the interpolated value
     490  // No checking of bounds here,   this function should be fast
          //
          
          #define INVSQRT2 0.707106781
          
          inline void
          NavFn::updateCellAstar(  int n )
          {
           // get neighbors
           float u,   d,   l,   r;
           l = potarr[n - 1];
           r = potarr[n + 1];
           u = potarr[n - nx];
           d = potarr[n + nx];
           // ROS_INFO(  "[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",  
           // potarr[n],   l,   r,   u,   d );
           // ROS_INFO(  "[Update] cost of %d: %d\n",   n,   costarr[n] );
          
           // find lowest,   and its lowest neighbor
           float ta,   tc;
           if (  l < r ) {tc = l;} else {tc = r;}
           if (  u < d ) {ta = u;} else {ta = d;}
          
           // do planar wave update
           if (  costarr[n] < COST_OBS ) { // don't propagate into obstacles
           float hf = static_cast<float>(  costarr[n] ); // traversability factor
           float dc = tc - ta; // relative cost between ta,  tc
           if (  dc < 0 ) { // ta is lowest
           dc = -dc;
           ta = tc;
           }
          
           // calculate new potential
           float pot;
           if (  dc >= hf ) { // if too large,   use ta-only update
           pot = ta + hf;
           } else { // two-neighbor interpolation update
           // use quadratic approximation
           // might speed this up through table lookup,   but still have to
           // do the divide
           float d = dc / hf;
           float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
           pot = ta + hf * v;
           }
          
           // ROS_INFO(  "[Update] new pot: %d\n",   costarr[n] );
          
           // now add affected neighbors to priority blocks
           if (  pot < potarr[n] ) {
           float le = INVSQRT2 * static_cast<float>(  costarr[n - 1] );
           float re = INVSQRT2 * static_cast<float>(  costarr[n + 1] );
           float ue = INVSQRT2 * static_cast<float>(  costarr[n - nx] );
           float de = INVSQRT2 * static_cast<float>(  costarr[n + nx] );
          
           // calculate distance
           int x = n % nx;
           int y = n / nx;
           float dist = hypot(  x - start[0],   y - start[1] ) * static_cast<float>(  COST_NEUTRAL );
          
           potarr[n] = pot;
           pot += dist;
           if (  pot < curT ) { // low-cost buffer block
           if (  l > pot + le ) {push_next(  n - 1 );}
           if (  r > pot + re ) {push_next(  n + 1 );}
           if (  u > pot + ue ) {push_next(  n - nx );}
           if (  d > pot + de ) {push_next(  n + nx );}
           } else {
           if (  l > pot + le ) {push_over(  n - 1 );}
           if (  r > pot + re ) {push_over(  n + 1 );}
           if (  u > pot + ue ) {push_over(  n - nx );}
           if (  d > pot + de ) {push_over(  n + nx );}
           }
           }
           }
          }
          
          
          //
          // main propagation function
          // Dijkstra method,   breadth-first
     570  // runs for a specified number of cycles,  
          // or until it runs out of cells to update,  
          // or until the Start cell is found (  atStart = true )
          //
          
          bool
          NavFn::propNavFnDijkstra(  int cycles,   bool atStart )
          {
           int nwv = 0; // max priority block size
           int nc = 0; // number of cells put into priority blocks
           int cycle = 0; // which cycle we're on
          
           // set up start cell
           int startCell = start[1] * nx + start[0];
          
           for (  ; cycle < cycles; cycle++ ) { // go for this many cycles,   unless interrupted
           if (  curPe == 0 && nextPe == 0 ) { // priority blocks empty
           break;
           }
          
           // stats
           nc += curPe;
           if (  curPe > nwv ) {
           nwv = curPe;
           }
          
           // reset pending flags on current priority buffer
           int * pb = curP;
           int i = curPe;
           while (  i-- > 0 ) {
           pending[*(  pb++ )] = false;
           }
          
           // process current priority buffer
           pb = curP;
           i = curPe;
           while (  i-- > 0 ) {
           updateCell(  *pb++ );
           }
          
           // if (  displayInt > 0 && (  cycle % displayInt ) == 0 ) {
           // displayFn(  this );
           // }
          
           // swap priority blocks curP <=> nextP
           curPe = nextPe;
           nextPe = 0;
           pb = curP; // swap buffers
           curP = nextP;
           nextP = pb;
          
           // see if we're done with this priority level
           if (  curPe == 0 ) {
           curT += priInc; // increment priority threshold
           curPe = overPe; // set current to overflow block
           overPe = 0;
           pb = curP; // swap buffers
           curP = overP;
           overP = pb;
           }
          
           // check if we've hit the Start cell
           if (  atStart ) {
           if (  potarr[startCell] < POT_HIGH ) {
           break;
           }
           }
           }
          
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),  
           "[NavFn] Used %d cycles,   %d cells visited (  %d%% ),   priority buf max %d\n",  
           cycle,   nc,   (  int )(  (  nc * 100.0 ) / (  ns - nobs ) ),   nwv );
          
           return (  cycle < cycles ) ? true : false;
          }
          
          //
          // main propagation function
          // A* method,   best-first
          // uses Euclidean distance heuristic
     651  // runs for a specified number of cycles,  
          // or until it runs out of cells to update,  
          // or until the Start cell is found (  atStart = true )
          //
          
          bool
          NavFn::propNavFnAstar(  int cycles )
          {
           int nwv = 0; // max priority block size
           int nc = 0; // number of cells put into priority blocks
           int cycle = 0; // which cycle we're on
          
           // set initial threshold,   based on distance
           float dist = hypot(  goal[0] - start[0],   goal[1] - start[1] ) * static_cast<float>(  COST_NEUTRAL );
           curT = dist + curT;
          
           // set up start cell
           int startCell = start[1] * nx + start[0];
          
           // do main cycle
           for (  ; cycle < cycles; cycle++ ) { // go for this many cycles,   unless interrupted
           if (  curPe == 0 && nextPe == 0 ) { // priority blocks empty
           break;
           }
          
           // stats
           nc += curPe;
           if (  curPe > nwv ) {
           nwv = curPe;
           }
          
           // reset pending flags on current priority buffer
           int * pb = curP;
           int i = curPe;
           while (  i-- > 0 ) {
           pending[*(  pb++ )] = false;
           }
          
           // process current priority buffer
           pb = curP;
           i = curPe;
           while (  i-- > 0 ) {
           updateCellAstar(  *pb++ );
           }
          
           // if (  displayInt > 0 && (  cycle % displayInt ) == 0 ) {
           // displayFn(  this );
           // }
          
           // swap priority blocks curP <=> nextP
           curPe = nextPe;
           nextPe = 0;
           pb = curP; // swap buffers
           curP = nextP;
           nextP = pb;
          
           // see if we're done with this priority level
           if (  curPe == 0 ) {
           curT += priInc; // increment priority threshold
           curPe = overPe; // set current to overflow block
           overPe = 0;
           pb = curP; // swap buffers
           curP = overP;
           overP = pb;
           }
          
           // check if we've hit the Start cell
           if (  potarr[startCell] < POT_HIGH ) {
           break;
           }
           }
          
           last_path_cost_ = potarr[startCell];
          
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),  
           "[NavFn] Used %d cycles,   %d cells visited (  %d%% ),   priority buf max %d\n",  
           cycle,   nc,   (  int )(  (  nc * 100.0 ) / (  ns - nobs ) ),   nwv );
          
           if (  potarr[startCell] < POT_HIGH ) {
           return true; // finished up here}
     732   } else {
           return false;
           }
          }
          
          
          float NavFn::getLastPathCost(   )
          {
           return last_path_cost_;
          }
          
          
          //
          // Path construction
          // Find gradient at array points,   interpolate path
          // Use step size of pathStep,   usually 0.5 pixel
          //
          // Some sanity checks:
     750  // 1. Stuck at same index position
          // 2. Doesn't get near goal
          // 3. Surrounded by high potentials
          //
          
          int
          NavFn::calcPath(  int n,   int * st )
          {
           // test write
           // savemap(  "test" );
          
           // check path arrays
           if (  npathbuf < n ) {
           if (  pathx ) {delete[] pathx;}
           if (  pathy ) {delete[] pathy;}
           pathx = new float[n];
           pathy = new float[n];
           npathbuf = n;
           }
          
           // set up start position at cell
           // st is always upper left corner for 4-point bilinear interpolation
           if (  st == NULL ) {st = start;}
           int stc = st[1] * nx + st[0];
          
           // set up offset
           float dx = 0;
           float dy = 0;
           npath = 0;
          
           // go for <n> cycles at most
           for (  int i = 0; i < n; i++ ) {
           // check if near goal
           int nearest_point = std::max(  
           0,  
           std::min(  
           nx * ny - 1,   stc + static_cast<int>(  round(  dx ) ) +
           static_cast<int>(  nx * round(  dy ) ) ) );
           if (  potarr[nearest_point] < COST_NEUTRAL ) {
           pathx[npath] = static_cast<float>(  goal[0] );
           pathy[npath] = static_cast<float>(  goal[1] );
           return ++npath; // done!
           }
          
           if (  stc < nx || stc > ns - nx ) { // would be out of bounds
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[PathCalc] Out of bounds" );
           return 0;
           }
          
           // add to path
           pathx[npath] = stc % nx + dx;
           pathy[npath] = stc / nx + dy;
           npath++;
          
           bool oscillation_detected = false;
           if (  npath > 2 &&
           pathx[npath - 1] == pathx[npath - 3] &&
           pathy[npath - 1] == pathy[npath - 3] )
           {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),  
           "[PathCalc] oscillation detected,   attempting fix." );
           oscillation_detected = true;
           }
          
           int stcnx = stc + nx;
           int stcpx = stc - nx;
          
           // check for potentials at eight positions near cell
           if (  potarr[stc] >= POT_HIGH ||
           potarr[stc + 1] >= POT_HIGH ||
           potarr[stc - 1] >= POT_HIGH ||
           potarr[stcnx] >= POT_HIGH ||
           potarr[stcnx + 1] >= POT_HIGH ||
           potarr[stcnx - 1] >= POT_HIGH ||
           potarr[stcpx] >= POT_HIGH ||
           potarr[stcpx + 1] >= POT_HIGH ||
           potarr[stcpx - 1] >= POT_HIGH ||
           oscillation_detected )
           {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),  
           "[Path] Pot fn boundary,   following grid (  %0.1f/%d )",   potarr[stc],   npath );
          
           // check eight neighbors to find the lowest
           int minc = stc;
           int minp = potarr[stc];
           int st = stcpx - 1;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st++;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st++;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st = stc - 1;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st = stc + 1;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st = stcnx - 1;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st++;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           st++;
           if (  potarr[st] < minp ) {minp = potarr[st]; minc = st;}
           stc = minc;
           dx = 0;
           dy = 0;
          
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),   "[Path] Pot: %0.1f pos: %0.1f,  %0.1f",  
           potarr[stc],   pathx[npath - 1],   pathy[npath - 1] );
          
           if (  potarr[stc] >= POT_HIGH ) {
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[PathCalc] No path found,   high potential" );
           // savemap(  "navfn_highpot" );
           return 0;
           }
           } else { // have a good gradient here
           // get grad at four positions near cell
           gradCell(  stc );
           gradCell(  stc + 1 );
           gradCell(  stcnx );
           gradCell(  stcnx + 1 );
          
          
           // get interpolated gradient
           float x1 = (  1.0 - dx ) * gradx[stc] + dx * gradx[stc + 1];
           float x2 = (  1.0 - dx ) * gradx[stcnx] + dx * gradx[stcnx + 1];
           float x = (  1.0 - dy ) * x1 + dy * x2; // interpolated x
           float y1 = (  1.0 - dx ) * grady[stc] + dx * grady[stc + 1];
           float y2 = (  1.0 - dx ) * grady[stcnx] + dx * grady[stcnx + 1];
           float y = (  1.0 - dy ) * y1 + dy * y2; // interpolated y
          
          #if 0
           // show gradients
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "rclcpp" ),  
           "[Path] %0.2f,  %0.2f %0.2f,  %0.2f %0.2f,  %0.2f %0.2f,  %0.2f; final x=%.3f,   y=%.3f\n",  
           gradx[stc],   grady[stc],   gradx[stc + 1],   grady[stc + 1],  
           gradx[stcnx],   grady[stcnx],   gradx[stcnx + 1],   grady[stcnx + 1],  
           x,   y );
          #endif
          
           // check for zero gradient,   failed
           if (  x == 0.0 && y == 0.0 ) {
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[PathCalc] Zero gradient" );
           return 0;
           }
          
           // move in the right direction
           float ss = pathStep / hypot(  x,   y );
           dx += x * ss;
           dy += y * ss;
          
           // check for overflow
           if (  dx > 1.0 ) {stc++; dx -= 1.0;}
           if (  dx < -1.0 ) {stc--; dx += 1.0;}
           if (  dy > 1.0 ) {stc += nx; dy -= 1.0;}
           if (  dy < -1.0 ) {stc -= nx; dy += 1.0;}
           }
          
           // ROS_INFO(  "[Path] Pot: %0.1f grad: %0.1f,  %0.1f pos: %0.1f,  %0.1f\n",  
           // potarr[stc],   x,   y,   pathx[npath-1],   pathy[npath-1] );
           }
          
           // return npath; // out of cycles,   return failure
           RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[PathCalc] No path found,   path too long" );
           // savemap(  "navfn_pathlong" );
           return 0; // out of cycles,   return failure
          }
          
          
          //
     922  // gradient calculations
          //
          
          // calculate gradient at a cell
          // positive value are to the right and down
          float
          NavFn::gradCell(  int n )
          {
           if (  gradx[n] + grady[n] > 0.0 ) { // check this cell
           return 1.0;
           }
          
           if (  n < nx || n > ns - nx ) { // would be out of bounds
           return 0.0;
           }
          
           float cv = potarr[n];
           float dx = 0.0;
           float dy = 0.0;
          
           // check for in an obstacle
           if (  cv >= POT_HIGH ) {
           if (  potarr[n - 1] < POT_HIGH ) {
           dx = -COST_OBS;
           } else if (  potarr[n + 1] < POT_HIGH ) {
           dx = COST_OBS;
           }
           if (  potarr[n - nx] < POT_HIGH ) {
           dy = -COST_OBS;
           } else if (  potarr[n + nx] < POT_HIGH ) {
           dy = COST_OBS;
           }
           } else { // not in an obstacle
           // dx calc,   average to sides
           if (  potarr[n - 1] < POT_HIGH ) {
           dx += potarr[n - 1] - cv;
           }
           if (  potarr[n + 1] < POT_HIGH ) {
           dx += cv - potarr[n + 1];
           }
          
           // dy calc,   average to sides
           if (  potarr[n - nx] < POT_HIGH ) {
           dy += potarr[n - nx] - cv;
           }
           if (  potarr[n + nx] < POT_HIGH ) {
           dy += cv - potarr[n + nx];
           }
           }
          
           // normalize
           float norm = hypot(  dx,   dy );
           if (  norm > 0 ) {
           norm = 1.0 / norm;
           gradx[n] = norm * dx;
           grady[n] = norm * dy;
           }
           return norm;
          }
          
          
          //
          // display function setup
          // <n> is the number of cycles to wait before displaying,  
          // use 0 to turn it off
          
          // void
          // NavFn::display(  void fn(  NavFn * nav ),   int n )
          // {
          // displayFn = fn;
          // displayInt = n;
          // }
          
          
          //
          // debug writes
          // saves costmap and start/goal
          //
          
          // void
          // NavFn::savemap(  const char * fname )
          // {
          // char fn[4096];
          
          // RCLCPP_DEBUG(  rclcpp::get_logger(  "rclcpp" ),   "[NavFn] Saving costmap and start/goal points" );
          // // write start and goal points
          // snprintf(  fn,   sizeof(  fn ),   "%s.txt",   fname );
          // FILE * fp = fopen(  fn,   "w" );
          // if (  !fp ) {
          // RCLCPP_WARN(  rclcpp::get_logger(  "rclcpp" ),   "Can't open file %s",   fn );
          // return;
          // }
          // fprintf(  fp,   "Goal: %d %d\nStart: %d %d\n",   goal[0],   goal[1],   start[0],   start[1] );
          // fclose(  fp );
          
          // // write cost array
          // if (  !costarr ) {
          // return;
          // }
          // snprintf(  fn,   sizeof(  fn ),   "%s.pgm",   fname );
          // fp = fopen(  fn,   "wb" );
          // if (  !fp ) {
          // RCLCPP_WARN(  rclcpp::get_logger(  "rclcpp" ),   "Can't open file %s",   fn );
          // return;
          // }
          // fprintf(  fp,   "P5\n%d\n%d\n%d\n",   nx,   ny,   0xff );
          // fwrite(  costarr,   1,   nx * ny,   fp );
          // fclose(  fp );
          // }
          
          } // namespace nav2_navfn_planner

navigation2/nav2_navfn_planner/src/navfn_planner.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2018 Simbe Robotics
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          // Navigation Strategy based on:
          // Brock,   O. and Oussama K. (  1999 ). High-Speed Navigation Using
          // the Global Dynamic Window Approach. IEEE.
          // https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
          
          // #define BENCHMARK_TESTING
          
          #include "nav2_navfn_planner/navfn_planner.hpp"
          
          #include <chrono>
          #include <cmath>
          #include <iomanip>
          #include <iostream>
          #include <limits>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "builtin_interfaces/msg/duration.hpp"
          #include "nav2_navfn_planner/navfn.hpp"
          #include "nav2_util/costmap.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          using nav2_util::declare_parameter_if_not_declared;
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_navfn_planner
          {
          
      50  NavfnPlanner::NavfnPlanner(   )
          : tf_(  nullptr ),   costmap_(  nullptr )
          {
          }
          
      55  NavfnPlanner::~NavfnPlanner(   )
          {
           RCLCPP_INFO(  
           logger_,   "Destroying plugin %s of type NavfnPlanner",  
           name_.c_str(   ) );
          }
          
          void
      63  NavfnPlanner::configure(  
      64   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      65   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      66   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           tf_ = tf;
           name_ = name;
           costmap_ = costmap_ros->getCostmap(   );
           global_frame_ = costmap_ros->getGlobalFrameID(   );
          
           node_ = parent;
           auto node = parent.lock(   );
           clock_ = node->get_clock(   );
           logger_ = node->get_logger(   );
          
           RCLCPP_INFO(  
           logger_,   "Configuring plugin %s of type NavfnPlanner",  
           name_.c_str(   ) );
          
           // Initialize parameters
           // Declare this plugin's parameters
           declare_parameter_if_not_declared(  node,   name + ".tolerance",   rclcpp::ParameterValue(  0.5 ) );
           node->get_parameter(  name + ".tolerance",   tolerance_ );
           declare_parameter_if_not_declared(  node,   name + ".use_astar",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".use_astar",   use_astar_ );
           declare_parameter_if_not_declared(  node,   name + ".allow_unknown",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".allow_unknown",   allow_unknown_ );
           declare_parameter_if_not_declared(  
           node,   name + ".use_final_approach_orientation",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".use_final_approach_orientation",   use_final_approach_orientation_ );
          
           // Create a planner based on the new costmap size
           planner_ = std::make_unique<NavFn>(  
           costmap_->getSizeInCellsX(   ),  
           costmap_->getSizeInCellsY(   ) );
          }
          
          void
     101  NavfnPlanner::activate(   )
          {
           RCLCPP_INFO(  
           logger_,   "Activating plugin %s of type NavfnPlanner",  
           name_.c_str(   ) );
           // Add callback for dynamic parameters
           auto node = node_.lock(   );
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &NavfnPlanner::dynamicParametersCallback,   this,   _1 ) );
          }
          
          void
     113  NavfnPlanner::deactivate(   )
          {
           RCLCPP_INFO(  
           logger_,   "Deactivating plugin %s of type NavfnPlanner",  
           name_.c_str(   ) );
           dyn_params_handler_.reset(   );
          }
          
          void
     122  NavfnPlanner::cleanup(   )
          {
           RCLCPP_INFO(  
           logger_,   "Cleaning up plugin %s of type NavfnPlanner",  
           name_.c_str(   ) );
           planner_.reset(   );
          }
          
     130  nav_msgs::msg::Path NavfnPlanner::createPlan(  
     131   const geometry_msgs::msg::PoseStamped & start,  
     132   const geometry_msgs::msg::PoseStamped & goal )
          {
          #ifdef BENCHMARK_TESTING
           steady_clock::time_point a = steady_clock::now(   );
          #endif
          
           // Update planner based on the new costmap size
           if (  isPlannerOutOfDate(   ) ) {
           planner_->setNavArr(  
           costmap_->getSizeInCellsX(   ),  
           costmap_->getSizeInCellsY(   ) );
           }
          
           nav_msgs::msg::Path path;
          
           // Corner case of the start(  x,  y ) = goal(  x,  y )
           if (  start.pose.position.x == goal.pose.position.x &&
           start.pose.position.y == goal.pose.position.y )
           {
           unsigned int mx,   my;
           costmap_->worldToMap(  start.pose.position.x,   start.pose.position.y,   mx,   my );
           if (  costmap_->getCost(  mx,   my ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           RCLCPP_WARN(  logger_,   "Failed to create a unique pose path because of obstacles" );
           return path;
           }
           path.header.stamp = clock_->now(   );
           path.header.frame_id = global_frame_;
           geometry_msgs::msg::PoseStamped pose;
           pose.header = path.header;
           pose.pose.position.z = 0.0;
          
           pose.pose = start.pose;
           // if we have a different start and goal orientation,   set the unique path pose to the goal
           // orientation,   unless use_final_approach_orientation=true where we need it to be the start
           // orientation to avoid movement from the local planner
           if (  start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_ ) {
           pose.pose.orientation = goal.pose.orientation;
           }
           path.poses.push_back(  pose );
           return path;
           }
          
           if (  !makePlan(  start.pose,   goal.pose,   tolerance_,   path ) ) {
           RCLCPP_WARN(  
           logger_,   "%s: failed to create plan with "
           "tolerance %.2f.",   name_.c_str(   ),   tolerance_ );
           }
          
          
          #ifdef BENCHMARK_TESTING
           steady_clock::time_point b = steady_clock::now(   );
           duration<double> time_span = duration_cast<duration<double>>(  b - a );
           std::cout << "It took " << time_span.count(   ) * 1000 << std::endl;
          #endif
          
           return path;
          }
          
          bool
     191  NavfnPlanner::isPlannerOutOfDate(   )
          {
           if (  !planner_.get(   ) ||
           planner_->nx != static_cast<int>(  costmap_->getSizeInCellsX(   ) ) ||
           planner_->ny != static_cast<int>(  costmap_->getSizeInCellsY(   ) ) )
           {
           return true;
           }
           return false;
          }
          
          bool
     203  NavfnPlanner::makePlan(  
     204   const geometry_msgs::msg::Pose & start,  
     205   const geometry_msgs::msg::Pose & goal,   double tolerance,  
     206   nav_msgs::msg::Path & plan )
          {
           // clear the plan,   just in case
           plan.poses.clear(   );
          
           plan.header.stamp = clock_->now(   );
           plan.header.frame_id = global_frame_;
          
           // TODO(  orduno ): add checks for start and goal reference frame -- should be in global frame
          
           double wx = start.position.x;
           double wy = start.position.y;
          
           RCLCPP_DEBUG(  
           logger_,   "Making plan from (  %.2f,  %.2f ) to (  %.2f,  %.2f )",  
           start.position.x,   start.position.y,   goal.position.x,   goal.position.y );
          
           unsigned int mx,   my;
           if (  !worldToMap(  wx,   wy,   mx,   my ) ) {
           RCLCPP_WARN(  
           logger_,  
           "Cannot create a plan: the robot's start position is off the global"
           " costmap. Planning will always fail,   are you sure"
           " the robot has been properly localized?" );
           return false;
           }
          
           // clear the starting cell within the costmap because we know it can't be an obstacle
           clearRobotCell(  mx,   my );
          
           std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(  *(  costmap_->getMutex(   ) ) );
          
           // make sure to resize the underlying array that Navfn uses
           planner_->setNavArr(  
           costmap_->getSizeInCellsX(   ),  
           costmap_->getSizeInCellsY(   ) );
          
           planner_->setCostmap(  costmap_->getCharMap(   ),   true,   allow_unknown_ );
          
           lock.unlock(   );
          
           int map_start[2];
           map_start[0] = mx;
           map_start[1] = my;
          
           wx = goal.position.x;
           wy = goal.position.y;
          
           if (  !worldToMap(  wx,   wy,   mx,   my ) ) {
           RCLCPP_WARN(  
           logger_,  
           "The goal sent to the planner is off the global costmap."
           " Planning will always fail to this goal." );
           return false;
           }
          
           int map_goal[2];
           map_goal[0] = mx;
           map_goal[1] = my;
          
           // TODO(  orduno ): Explain why we are providing 'map_goal' to setStart(   ).
           // Same for setGoal,   seems reversed. Computing backwards?
          
           planner_->setStart(  map_goal );
           planner_->setGoal(  map_start );
           if (  use_astar_ ) {
           planner_->calcNavFnAstar(   );
           } else {
           planner_->calcNavFnDijkstra(  true );
           }
          
           double resolution = costmap_->getResolution(   );
           geometry_msgs::msg::Pose p,   best_pose;
          
           bool found_legal = false;
          
           p = goal;
           double potential = getPointPotential(  p.position );
           if (  potential < POT_HIGH ) {
           // Goal is reachable by itself
           best_pose = p;
           found_legal = true;
           } else {
           // Goal is not reachable. Trying to find nearest to the goal
           // reachable point within its tolerance region
           double best_sdist = std::numeric_limits<double>::max(   );
          
           p.position.y = goal.position.y - tolerance;
           while (  p.position.y <= goal.position.y + tolerance ) {
           p.position.x = goal.position.x - tolerance;
           while (  p.position.x <= goal.position.x + tolerance ) {
           potential = getPointPotential(  p.position );
           double sdist = squared_distance(  p,   goal );
           if (  potential < POT_HIGH && sdist < best_sdist ) {
           best_sdist = sdist;
           best_pose = p;
           found_legal = true;
           }
           p.position.x += resolution;
           }
           p.position.y += resolution;
           }
           }
          
           if (  found_legal ) {
           // extract the plan
           if (  getPlanFromPotential(  best_pose,   plan ) ) {
           smoothApproachToGoal(  best_pose,   plan );
          
           // If use_final_approach_orientation=true,   interpolate the last pose orientation from the
           // previous pose to set the orientation to the 'final approach' orientation of the robot so
           // it does not rotate.
           // And deal with corner case of plan of length 1
           if (  use_final_approach_orientation_ ) {
           size_t plan_size = plan.poses.size(   );
           if (  plan_size == 1 ) {
           plan.poses.back(   ).pose.orientation = start.orientation;
           } else if (  plan_size > 1 ) {
           double dx,   dy,   theta;
           auto last_pose = plan.poses.back(   ).pose.position;
           auto approach_pose = plan.poses[plan_size - 2].pose.position;
           // Deal with the case of NavFn producing a path with two equal last poses
           if (  std::abs(  last_pose.x - approach_pose.x ) < 0.0001 &&
           std::abs(  last_pose.y - approach_pose.y ) < 0.0001 && plan_size > 2 )
           {
           approach_pose = plan.poses[plan_size - 3].pose.position;
           }
           dx = last_pose.x - approach_pose.x;
           dy = last_pose.y - approach_pose.y;
           theta = atan2(  dy,   dx );
           plan.poses.back(   ).pose.orientation =
           nav2_util::geometry_utils::orientationAroundZAxis(  theta );
           }
           }
           } else {
           RCLCPP_ERROR(  
           logger_,  
           "Failed to create a plan from potential when a legal"
           " potential was found. This shouldn't happen." );
           }
           }
          
           return !plan.poses.empty(   );
          }
          
          void
     352  NavfnPlanner::smoothApproachToGoal(  
     353   const geometry_msgs::msg::Pose & goal,  
     354   nav_msgs::msg::Path & plan )
          {
           // Replace the last pose of the computed path if it's actually further away
           // to the second to last pose than the goal pose.
           if (  plan.poses.size(   ) >= 2 ) {
           auto second_to_last_pose = plan.poses.end(   )[-2];
           auto last_pose = plan.poses.back(   );
           if (  
           squared_distance(  last_pose.pose,   second_to_last_pose.pose ) >
           squared_distance(  goal,   second_to_last_pose.pose ) )
           {
           plan.poses.back(   ).pose = goal;
           return;
           }
           }
           geometry_msgs::msg::PoseStamped goal_copy;
           goal_copy.pose = goal;
           plan.poses.push_back(  goal_copy );
          }
          
          bool
     375  NavfnPlanner::getPlanFromPotential(  
     376   const geometry_msgs::msg::Pose & goal,  
     377   nav_msgs::msg::Path & plan )
          {
           // clear the plan,   just in case
           plan.poses.clear(   );
          
           // Goal should be in global frame
           double wx = goal.position.x;
           double wy = goal.position.y;
          
           // the potential has already been computed,   so we won't update our copy of the costmap
           unsigned int mx,   my;
           if (  !worldToMap(  wx,   wy,   mx,   my ) ) {
           RCLCPP_WARN(  
           logger_,  
           "The goal sent to the navfn planner is off the global costmap."
           " Planning will always fail to this goal." );
           return false;
           }
          
           int map_goal[2];
           map_goal[0] = mx;
           map_goal[1] = my;
          
           planner_->setStart(  map_goal );
          
           const int & max_cycles = (  costmap_->getSizeInCellsX(   ) >= costmap_->getSizeInCellsY(   ) ) ?
           (  costmap_->getSizeInCellsX(   ) * 4 ) : (  costmap_->getSizeInCellsY(   ) * 4 );
          
           int path_len = planner_->calcPath(  max_cycles );
           if (  path_len == 0 ) {
           return false;
           }
          
           auto cost = planner_->getLastPathCost(   );
           RCLCPP_DEBUG(  
           logger_,  
           "Path found,   %d steps,   %f cost\n",   path_len,   cost );
          
           // extract the plan
           float * x = planner_->getPathX(   );
           float * y = planner_->getPathY(   );
           int len = planner_->getPathLen(   );
          
           for (  int i = len - 1; i >= 0; --i ) {
           // convert the plan to world coordinates
           double world_x,   world_y;
           mapToWorld(  x[i],   y[i],   world_x,   world_y );
          
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = world_x;
           pose.pose.position.y = world_y;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
           plan.poses.push_back(  pose );
           }
          
           return !plan.poses.empty(   );
          }
          
          double
     440  NavfnPlanner::getPointPotential(  const geometry_msgs::msg::Point & world_point )
          {
           unsigned int mx,   my;
           if (  !worldToMap(  world_point.x,   world_point.y,   mx,   my ) ) {
           return std::numeric_limits<double>::max(   );
           }
          
           unsigned int index = my * planner_->nx + mx;
           return planner_->potarr[index];
          }
          
          // bool
          // NavfnPlanner::validPointPotential(  const geometry_msgs::msg::Point & world_point )
          // {
          // return validPointPotential(  world_point,   tolerance_ );
          // }
          
          // bool
          // NavfnPlanner::validPointPotential(  
          // const geometry_msgs::msg::Point & world_point,   double tolerance )
          // {
          // const double resolution = costmap_->getResolution(   );
          
          // geometry_msgs::msg::Point p = world_point;
          // double potential = getPointPotential(  p );
          // if (  potential < POT_HIGH ) {
          // // world_point is reachable by itself
          // return true;
          // } else {
          // // world_point,   is not reachable. Trying to find any
          // // reachable point within its tolerance region
          // p.y = world_point.y - tolerance;
          // while (  p.y <= world_point.y + tolerance ) {
          // p.x = world_point.x - tolerance;
          // while (  p.x <= world_point.x + tolerance ) {
          // potential = getPointPotential(  p );
          // if (  potential < POT_HIGH ) {
          // return true;
          // }
          // p.x += resolution;
          // }
          // p.y += resolution;
          // }
          // }
          
          // return false;
          // }
          
          bool
     489  NavfnPlanner::worldToMap(  double wx,   double wy,   unsigned int & mx,   unsigned int & my )
          {
           if (  wx < costmap_->getOriginX(   ) || wy < costmap_->getOriginY(   ) ) {
           return false;
           }
          
           mx = static_cast<int>(  
           std::round(  (  wx - costmap_->getOriginX(   ) ) / costmap_->getResolution(   ) ) );
           my = static_cast<int>(  
           std::round(  (  wy - costmap_->getOriginY(   ) ) / costmap_->getResolution(   ) ) );
          
           if (  mx < costmap_->getSizeInCellsX(   ) && my < costmap_->getSizeInCellsY(   ) ) {
           return true;
           }
          
           RCLCPP_ERROR(  
           logger_,  
           "worldToMap failed: mx,  my: %d,  %d,   size_x,  size_y: %d,  %d",   mx,   my,  
           costmap_->getSizeInCellsX(   ),   costmap_->getSizeInCellsY(   ) );
          
           return false;
          }
          
          void
     513  NavfnPlanner::mapToWorld(  double mx,   double my,   double & wx,   double & wy )
          {
           wx = costmap_->getOriginX(   ) + mx * costmap_->getResolution(   );
           wy = costmap_->getOriginY(   ) + my * costmap_->getResolution(   );
          }
          
          void
     520  NavfnPlanner::clearRobotCell(  unsigned int mx,   unsigned int my )
          {
           // TODO(  orduno ): check usage of this function,   might instead be a request to
           // world_model / map server
           costmap_->setCost(  mx,   my,   nav2_costmap_2d::FREE_SPACE );
          }
          
          rcl_interfaces::msg::SetParametersResult
     528  NavfnPlanner::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == name_ + ".tolerance" ) {
           tolerance_ = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == name_ + ".use_astar" ) {
           use_astar_ = parameter.as_bool(   );
           } else if (  name == name_ + ".allow_unknown" ) {
           allow_unknown_ = parameter.as_bool(   );
           } else if (  name == name_ + ".use_final_approach_orientation" ) {
           use_final_approach_orientation_ = parameter.as_bool(   );
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_navfn_planner
          
          #include "pluginlib/class_list_macros.hpp"
     556  PLUGINLIB_EXPORT_CLASS(  nav2_navfn_planner::NavfnPlanner,   nav2_core::GlobalPlanner )

navigation2/nav2_navfn_planner/test/test_dynamic_parameters.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_navfn_planner/navfn_planner.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      26  class RclCppFixture
          {
          public:
      29   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      30   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      34  TEST(  NavfnTest,   testDynamicParameter )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "Navfntest" );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
           auto planner =
           std::make_unique<nav2_navfn_planner::NavfnPlanner>(   );
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           planner->configure(  node,   "test",   tf,   costmap );
           planner->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           node->get_node_base_interface(   ),   node->get_node_topics_interface(   ),  
           node->get_node_graph_interface(   ),  
           node->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.tolerance",   1.0 ),  
           rclcpp::Parameter(  "test.use_astar",   true ),  
           rclcpp::Parameter(  "test.allow_unknown",   true ),  
           rclcpp::Parameter(  "test.use_final_approach_orientation",   true )} );
          
           rclcpp::spin_until_future_complete(  
           node->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  node->get_parameter(  "test.tolerance" ).as_double(   ),   1.0 );
           EXPECT_EQ(  node->get_parameter(  "test.use_astar" ).as_bool(   ),   true );
           EXPECT_EQ(  node->get_parameter(  "test.allow_unknown" ).as_bool(   ),   true );
           EXPECT_EQ(  node->get_parameter(  "test.use_final_approach_orientation" ).as_bool(   ),   true );
          }

navigation2/nav2_planner/include/nav2_planner/planner_server.hpp

          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_PLANNER__PLANNER_SERVER_HPP_
          #define NAV2_PLANNER__PLANNER_SERVER_HPP_
          
          #include <chrono>
          #include <string>
          #include <memory>
          #include <vector>
          #include <unordered_map>
          #include <mutex>
          
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_msgs/action/compute_path_to_pose.hpp"
          #include "nav2_msgs/action/compute_path_through_poses.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "visualization_msgs/msg/marker.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav2_msgs/srv/is_path_valid.hpp"
          
          namespace nav2_planner
          {
          /**
           * @class nav2_planner::PlannerServer
           * @brief An action server implements the behavior tree's ComputePathToPose
           * interface and hosts various plugins of different algorithms to compute plans.
           */
      50  class PlannerServer : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief A constructor for nav2_planner::PlannerServer
           * @param options Additional options to control creation of the node.
           */
      57   explicit PlannerServer(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief A destructor for nav2_planner::PlannerServer
           */
      61   ~PlannerServer(   );
          
           using PlannerMap = std::unordered_map<std::string,   nav2_core::GlobalPlanner::Ptr>;
          
           /**
           * @brief Method to get plan from the desired plugin
           * @param start starting pose
           * @param goal goal request
           * @return Path
           */
           nav_msgs::msg::Path getPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal,  
           const std::string & planner_id );
          
          protected:
           /**
           * @brief Configure member variables and initializes planner
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Activate member variables
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Deactivate member variables
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Reset member variables
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in shutdown state
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           using ActionToPose = nav2_msgs::action::ComputePathToPose;
           using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
           using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
           using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;
          
           /**
           * @brief Check if an action server is valid / active
           * @param action_server Action server to test
           * @return SUCCESS or FAILURE
           */
           template<typename T>
           bool isServerInactive(  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server );
          
           /**
           * @brief Check if an action server has a cancellation request pending
           * @param action_server Action server to test
           * @return SUCCESS or FAILURE
           */
           template<typename T>
           bool isCancelRequested(  std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server );
          
           /**
           * @brief Wait for costmap to be valid with updated sensor data or repopulate after a
           * clearing recovery. Blocks until true without timeout.
           */
           void waitForCostmap(   );
          
           /**
           * @brief Check if an action server has a preemption request and replaces the goal
           * with the new preemption goal.
           * @param action_server Action server to get updated goal if required
           * @param goal Goal to overwrite
           */
           template<typename T>
           void getPreemptedGoalIfRequested(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           typename std::shared_ptr<const typename T::Goal> goal );
          
           /**
           * @brief Get the starting pose from costmap or message,   if valid
           * @param action_server Action server to terminate if required
           * @param goal Goal to find start from
           * @param start The starting pose to use
           * @return bool If successful in finding a valid starting pose
           */
           template<typename T>
           bool getStartPose(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           typename std::shared_ptr<const typename T::Goal> goal,  
           geometry_msgs::msg::PoseStamped & start );
          
           /**
           * @brief Transform start and goal poses into the costmap
           * global frame for path planning plugins to utilize
           * @param action_server Action server to terminate if required
           * @param start The starting pose to transform
           * @param goal Goal pose to transform
           * @return bool If successful in transforming poses
           */
           template<typename T>
           bool transformPosesToGlobalFrame(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           geometry_msgs::msg::PoseStamped & curr_start,  
           geometry_msgs::msg::PoseStamped & curr_goal );
          
           /**
           * @brief Validate that the path contains a meaningful path
           * @param action_server Action server to terminate if required
           * @param goal Goal Current goal
           * @param path Current path
           * @param planner_id The planner ID used to generate the path
           * @return bool If path is valid
           */
           template<typename T>
           bool validatePath(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           const geometry_msgs::msg::PoseStamped & curr_goal,  
           const nav_msgs::msg::Path & path,  
           const std::string & planner_id );
          
           // Our action server implements the ComputePathToPose action
           std::unique_ptr<ActionServerToPose> action_server_pose_;
           std::unique_ptr<ActionServerThroughPoses> action_server_poses_;
          
           /**
           * @brief The action server callback which calls planner to get the path
           * ComputePathToPose
           */
           void computePlan(   );
          
           /**
           * @brief The action server callback which calls planner to get the path
           * ComputePathThroughPoses
           */
           void computePlanThroughPoses(   );
          
           /**
           * @brief The service callback to determine if the path is still valid
           * @param request to the service
           * @param response from the service
           */
           void isPathValid(  
           const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,  
           std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response );
          
           /**
           * @brief Publish a path for visualization purposes
           * @param path Reference to Global Path
           */
           void publishPlan(  const nav_msgs::msg::Path & path );
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
           std::mutex dynamic_params_lock_;
          
           // Planner
           PlannerMap planners_;
           pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
           std::vector<std::string> default_ids_;
           std::vector<std::string> default_types_;
           std::vector<std::string> planner_ids_;
           std::vector<std::string> planner_types_;
           double max_planner_duration_;
           std::string planner_ids_concat_;
          
           // Clock
           rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
          
           // TF buffer
           std::shared_ptr<tf2_ros::Buffer> tf_;
          
           // Global Costmap
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
           std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
           nav2_costmap_2d::Costmap2D * costmap_;
          
           // Publishers for the path
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
          
           // Service to deterime if the path is valid
           rclcpp::Service<nav2_msgs::srv::IsPathValid>::SharedPtr is_path_valid_service_;
          };
          
          } // namespace nav2_planner
          
          #endif // NAV2_PLANNER__PLANNER_SERVER_HPP_

navigation2/nav2_planner/src/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_planner/planner_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      21  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_planner::PlannerServer>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_planner/src/planner_server.cpp

          // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <cmath>
          #include <iomanip>
          #include <iostream>
          #include <limits>
          #include <iterator>
          #include <memory>
          #include <string>
          #include <vector>
          #include <utility>
          
          #include "builtin_interfaces/msg/duration.hpp"
          #include "nav2_util/costmap.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          
          #include "nav2_planner/planner_server.hpp"
          
          using namespace std::chrono_literals;
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
          namespace nav2_planner
          {
          
      42  PlannerServer::PlannerServer(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "planner_server",   "",   options ),  
           gp_loader_(  "nav2_core",   "nav2_core::GlobalPlanner" ),  
           default_ids_{"GridBased"},  
      46   default_types_{"nav2_navfn_planner/NavfnPlanner"},  
      47   costmap_(  nullptr )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           // Declare this node's parameters
           declare_parameter(  "planner_plugins",   default_ids_ );
           declare_parameter(  "expected_planner_frequency",   1.0 );
          
           get_parameter(  "planner_plugins",   planner_ids_ );
           if (  planner_ids_ == default_ids_ ) {
           for (  size_t i = 0; i < default_ids_.size(   ); ++i ) {
           declare_parameter(  default_ids_[i] + ".plugin",   default_types_[i] );
           }
           }
          
           // Setup the global costmap
           costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  
           "global_costmap",   std::string{get_namespace(   )},   "global_costmap" );
          
           // Launch a thread to run the costmap node
           costmap_thread_ = std::make_unique<nav2_util::NodeThread>(  costmap_ros_ );
          }
          
      70  PlannerServer::~PlannerServer(   )
          {
           planners_.clear(   );
           costmap_thread_.reset(   );
          }
          
          nav2_util::CallbackReturn
      77  PlannerServer::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           costmap_ros_->configure(   );
           costmap_ = costmap_ros_->getCostmap(   );
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "Costmap size: %d,  %d",  
           costmap_->getSizeInCellsX(   ),   costmap_->getSizeInCellsY(   ) );
          
           tf_ = costmap_ros_->getTfBuffer(   );
          
           planner_types_.resize(  planner_ids_.size(   ) );
          
           auto node = shared_from_this(   );
          
           for (  size_t i = 0; i != planner_ids_.size(   ); i++ ) {
           try {
           planner_types_[i] = nav2_util::get_plugin_type_param(  
           node,   planner_ids_[i] );
           nav2_core::GlobalPlanner::Ptr planner =
           gp_loader_.createUniqueInstance(  planner_types_[i] );
           RCLCPP_INFO(  
           get_logger(   ),   "Created global planner plugin %s of type %s",  
           planner_ids_[i].c_str(   ),   planner_types_[i].c_str(   ) );
           planner->configure(  node,   planner_ids_[i],   tf_,   costmap_ros_ );
           planners_.insert(  {planner_ids_[i],   planner} );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),   "Failed to create global planner. Exception: %s",  
           ex.what(   ) );
           return nav2_util::CallbackReturn::FAILURE;
           }
           }
          
           for (  size_t i = 0; i != planner_ids_.size(   ); i++ ) {
           planner_ids_concat_ += planner_ids_[i] + std::string(  " " );
           }
          
           RCLCPP_INFO(  
           get_logger(   ),  
           "Planner Server has %s planners available.",   planner_ids_concat_.c_str(   ) );
          
           double expected_planner_frequency;
           get_parameter(  "expected_planner_frequency",   expected_planner_frequency );
           if (  expected_planner_frequency > 0 ) {
           max_planner_duration_ = 1 / expected_planner_frequency;
           } else {
           RCLCPP_WARN(  
           get_logger(   ),  
           "The expected planner frequency parameter is %.4f Hz. The value should to be greater"
           " than 0.0 to turn on duration overrrun warning messages",   expected_planner_frequency );
           max_planner_duration_ = 0.0;
           }
          
           // Initialize pubs & subs
           plan_publisher_ = create_publisher<nav_msgs::msg::Path>(  "plan",   1 );
          
           // Create the action servers for path planning to a pose and through poses
           action_server_pose_ = std::make_unique<ActionServerToPose>(  
           shared_from_this(   ),  
           "compute_path_to_pose",  
           std::bind(  &PlannerServer::computePlan,   this ),  
           nullptr,  
           std::chrono::milliseconds(  500 ),  
           true );
          
           action_server_poses_ = std::make_unique<ActionServerThroughPoses>(  
           shared_from_this(   ),  
           "compute_path_through_poses",  
           std::bind(  &PlannerServer::computePlanThroughPoses,   this ),  
           nullptr,  
           std::chrono::milliseconds(  500 ),  
           true );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     157  PlannerServer::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           plan_publisher_->on_activate(   );
           action_server_pose_->activate(   );
           action_server_poses_->activate(   );
           costmap_ros_->activate(   );
          
           PlannerMap::iterator it;
           for (  it = planners_.begin(   ); it != planners_.end(   ); ++it ) {
           it->second->activate(   );
           }
          
           auto node = shared_from_this(   );
          
           is_path_valid_service_ = node->create_service<nav2_msgs::srv::IsPathValid>(  
           "is_path_valid",  
           std::bind(  
           &PlannerServer::isPathValid,   this,  
           std::placeholders::_1,   std::placeholders::_2 ) );
          
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &PlannerServer::dynamicParametersCallback,   this,   _1 ) );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     190  PlannerServer::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           action_server_pose_->deactivate(   );
           action_server_poses_->deactivate(   );
           plan_publisher_->on_deactivate(   );
           costmap_ros_->deactivate(   );
          
           PlannerMap::iterator it;
           for (  it = planners_.begin(   ); it != planners_.end(   ); ++it ) {
           it->second->deactivate(   );
           }
          
           dyn_params_handler_.reset(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     213  PlannerServer::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           action_server_pose_.reset(   );
           action_server_poses_.reset(   );
           plan_publisher_.reset(   );
           tf_.reset(   );
           costmap_ros_->cleanup(   );
          
           PlannerMap::iterator it;
           for (  it = planners_.begin(   ); it != planners_.end(   ); ++it ) {
           it->second->cleanup(   );
           }
           planners_.clear(   );
           costmap_ = nullptr;
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     233  PlannerServer::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          template<typename T>
     240  bool PlannerServer::isServerInactive(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server )
          {
           if (  action_server == nullptr || !action_server->is_server_active(   ) ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Action server unavailable or inactive. Stopping." );
           return true;
           }
          
           return false;
          }
          
     251  void PlannerServer::waitForCostmap(   )
          {
           // Don't compute a plan until costmap is valid (  after clear costmap )
           rclcpp::Rate r(  100 );
           while (  !costmap_ros_->isCurrent(   ) ) {
           r.sleep(   );
           }
          }
          
          template<typename T>
     261  bool PlannerServer::isCancelRequested(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server )
          {
           if (  action_server->is_cancel_requested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Goal was canceled. Canceling planning action." );
           action_server->terminate_all(   );
           return true;
           }
          
           return false;
          }
          
          template<typename T>
     274  void PlannerServer::getPreemptedGoalIfRequested(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           typename std::shared_ptr<const typename T::Goal> goal )
          {
           if (  action_server->is_preempt_requested(   ) ) {
           goal = action_server->accept_pending_goal(   );
           }
          }
          
          template<typename T>
     284  bool PlannerServer::getStartPose(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           typename std::shared_ptr<const typename T::Goal> goal,  
           geometry_msgs::msg::PoseStamped & start )
          {
           if (  goal->use_start ) {
           start = goal->start;
           } else if (  !costmap_ros_->getRobotPose(  start ) ) {
           action_server->terminate_current(   );
           return false;
           }
          
           return true;
          }
          
          template<typename T>
     300  bool PlannerServer::transformPosesToGlobalFrame(  
           std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           geometry_msgs::msg::PoseStamped & curr_start,  
           geometry_msgs::msg::PoseStamped & curr_goal )
          {
           if (  !costmap_ros_->transformPoseToGlobalFrame(  curr_start,   curr_start ) ||
           !costmap_ros_->transformPoseToGlobalFrame(  curr_goal,   curr_goal ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),   "Could not transform the start or goal pose in the costmap frame" );
           action_server->terminate_current(   );
           return false;
           }
          
           return true;
          }
          
          template<typename T>
     318  bool PlannerServer::validatePath(  
     319   std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,  
           const geometry_msgs::msg::PoseStamped & goal,  
           const nav_msgs::msg::Path & path,  
           const std::string & planner_id )
          {
           if (  path.poses.size(   ) == 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),   "Planning algorithm %s failed to generate a valid"
           " path to (  %.2f,   %.2f )",   planner_id.c_str(   ),  
           goal.pose.position.x,   goal.pose.position.y );
           action_server->terminate_current(   );
           return false;
           }
          
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Found valid path of size %zu to (  %.2f,   %.2f )",  
           path.poses.size(   ),   goal.pose.position.x,  
           goal.pose.position.y );
          
           return true;
          }
          
          void
          PlannerServer::computePlanThroughPoses(   )
          {
           std::lock_guard<std::mutex> lock(  dynamic_params_lock_ );
          
           auto start_time = steady_clock_.now(   );
          
           // Initialize the ComputePathToPose goal and result
           auto goal = action_server_poses_->get_current_goal(   );
           auto result = std::make_shared<ActionThroughPoses::Result>(   );
           nav_msgs::msg::Path concat_path;
          
           try {
           if (  isServerInactive(  action_server_poses_ ) || isCancelRequested(  action_server_poses_ ) ) {
           return;
           }
          
           waitForCostmap(   );
          
           getPreemptedGoalIfRequested(  action_server_poses_,   goal );
          
           if (  goal->goals.size(   ) == 0 ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Compute path through poses requested a plan with no viapoint poses,   returning." );
           action_server_poses_->terminate_current(   );
           }
          
           // Use start pose if provided otherwise use current robot pose
           geometry_msgs::msg::PoseStamped start;
           if (  !getStartPose(  action_server_poses_,   goal,   start ) ) {
           return;
           }
          
           // Get consecutive paths through these points
           std::vector<geometry_msgs::msg::PoseStamped>::iterator goal_iter;
           geometry_msgs::msg::PoseStamped curr_start,   curr_goal;
           for (  unsigned int i = 0; i != goal->goals.size(   ); i++ ) {
           // Get starting point
           if (  i == 0 ) {
           curr_start = start;
           } else {
           curr_start = goal->goals[i - 1];
           }
           curr_goal = goal->goals[i];
          
           // Transform them into the global frame
           if (  !transformPosesToGlobalFrame(  action_server_poses_,   curr_start,   curr_goal ) ) {
           return;
           }
          
           // Get plan from start -> goal
           nav_msgs::msg::Path curr_path = getPlan(  curr_start,   curr_goal,   goal->planner_id );
          
           // check path for validity
           if (  !validatePath(  action_server_poses_,   curr_goal,   curr_path,   goal->planner_id ) ) {
           return;
           }
          
           // Concatenate paths together
           concat_path.poses.insert(  
           concat_path.poses.end(   ),   curr_path.poses.begin(   ),   curr_path.poses.end(   ) );
           concat_path.header = curr_path.header;
           }
          
           // Publish the plan for visualization purposes
           result->path = concat_path;
           publishPlan(  result->path );
          
           auto cycle_duration = steady_clock_.now(   ) - start_time;
           result->planning_time = cycle_duration;
          
           if (  max_planner_duration_ && cycle_duration.seconds(   ) > max_planner_duration_ ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",  
           1 / max_planner_duration_,   1 / cycle_duration.seconds(   ) );
           }
          
           action_server_poses_->succeeded_current(  result );
           } catch (  std::exception & ex ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "%s plugin failed to plan through %zu points with final goal (  %.2f,   %.2f ): \"%s\"",  
           goal->planner_id.c_str(   ),   goal->goals.size(   ),   goal->goals.back(   ).pose.position.x,  
           goal->goals.back(   ).pose.position.y,   ex.what(   ) );
           action_server_poses_->terminate_current(   );
           }
          }
          
          void
          PlannerServer::computePlan(   )
          {
           std::lock_guard<std::mutex> lock(  dynamic_params_lock_ );
          
           auto start_time = steady_clock_.now(   );
          
           // Initialize the ComputePathToPose goal and result
           auto goal = action_server_pose_->get_current_goal(   );
           auto result = std::make_shared<ActionToPose::Result>(   );
          
           try {
           if (  isServerInactive(  action_server_pose_ ) || isCancelRequested(  action_server_pose_ ) ) {
           return;
           }
          
           waitForCostmap(   );
          
           getPreemptedGoalIfRequested(  action_server_pose_,   goal );
          
           // Use start pose if provided otherwise use current robot pose
           geometry_msgs::msg::PoseStamped start;
           if (  !getStartPose(  action_server_pose_,   goal,   start ) ) {
           return;
           }
          
           // Transform them into the global frame
           geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
           if (  !transformPosesToGlobalFrame(  action_server_pose_,   start,   goal_pose ) ) {
           return;
           }
          
           result->path = getPlan(  start,   goal_pose,   goal->planner_id );
          
           if (  !validatePath(  action_server_pose_,   goal_pose,   result->path,   goal->planner_id ) ) {
           return;
           }
          
           // Publish the plan for visualization purposes
           publishPlan(  result->path );
          
           auto cycle_duration = steady_clock_.now(   ) - start_time;
           result->planning_time = cycle_duration;
          
           if (  max_planner_duration_ && cycle_duration.seconds(   ) > max_planner_duration_ ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",  
           1 / max_planner_duration_,   1 / cycle_duration.seconds(   ) );
           }
          
     483   action_server_pose_->succeeded_current(  result );
           } catch (  std::exception & ex ) {
           RCLCPP_WARN(  
           get_logger(   ),   "%s plugin failed to plan calculation to (  %.2f,   %.2f ): \"%s\"",  
           goal->planner_id.c_str(   ),   goal->goal.pose.position.x,  
           goal->goal.pose.position.y,   ex.what(   ) );
           action_server_pose_->terminate_current(   );
           }
          }
          
          nav_msgs::msg::Path
          PlannerServer::getPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal,  
           const std::string & planner_id )
          {
           RCLCPP_DEBUG(  
           get_logger(   ),   "Attempting to a find path from (  %.2f,   %.2f ) to "
           "(  %.2f,   %.2f ).",   start.pose.position.x,   start.pose.position.y,  
           goal.pose.position.x,   goal.pose.position.y );
          
           if (  planners_.find(  planner_id ) != planners_.end(   ) ) {
           return planners_[planner_id]->createPlan(  start,   goal );
           } else {
           if (  planners_.size(   ) == 1 && planner_id.empty(   ) ) {
           RCLCPP_WARN_ONCE(  
           get_logger(   ),   "No planners specified in action call. "
           "Server will use only plugin %s in server."
           " This warning will appear once.",   planner_ids_concat_.c_str(   ) );
           return planners_[planners_.begin(   )->first]->createPlan(  start,   goal );
           } else {
           RCLCPP_ERROR(  
           get_logger(   ),   "planner %s is not a valid planner. "
           "Planner names are: %s",   planner_id.c_str(   ),  
           planner_ids_concat_.c_str(   ) );
           }
           }
          
           return nav_msgs::msg::Path(   );
          }
          
          void
          PlannerServer::publishPlan(  const nav_msgs::msg::Path & path )
          {
           auto msg = std::make_unique<nav_msgs::msg::Path>(  path );
           if (  plan_publisher_->is_activated(   ) && plan_publisher_->get_subscription_count(   ) > 0 ) {
           plan_publisher_->publish(  std::move(  msg ) );
           }
          }
          
          void PlannerServer::isPathValid(  
           const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,  
           std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response )
          {
           response->is_valid = true;
          
           if (  request->path.poses.empty(   ) ) {
           response->is_valid = false;
           return;
           }
          
           geometry_msgs::msg::PoseStamped current_pose;
           unsigned int closest_point_index = 0;
           if (  costmap_ros_->getRobotPose(  current_pose ) ) {
           float current_distance = std::numeric_limits<float>::max(   );
           float closest_distance = current_distance;
           geometry_msgs::msg::Point current_point = current_pose.pose.position;
           for (  unsigned int i = 0; i < request->path.poses.size(   ); ++i ) {
           geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position;
          
           current_distance = nav2_util::geometry_utils::euclidean_distance(  
           current_point,  
           path_point );
          
           if (  current_distance < closest_distance ) {
           closest_point_index = i;
           closest_distance = current_distance;
           }
           }
          
           /**
           * The lethal check starts at the closest point to avoid points that have already been passed
           * and may have become occupied
           */
           unsigned int mx = 0;
           unsigned int my = 0;
           for (  unsigned int i = closest_point_index; i < request->path.poses.size(   ); ++i ) {
           costmap_->worldToMap(  
           request->path.poses[i].pose.position.x,  
           request->path.poses[i].pose.position.y,   mx,   my );
           unsigned int cost = costmap_->getCost(  mx,   my );
          
           if (  cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
           cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
           {
           response->is_valid = false;
           }
           }
           }
          }
          
          rcl_interfaces::msg::SetParametersResult
          PlannerServer::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           std::lock_guard<std::mutex> lock(  dynamic_params_lock_ );
           rcl_interfaces::msg::SetParametersResult result;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == "expected_planner_frequency" ) {
           if (  parameter.as_double(   ) > 0 ) {
           max_planner_duration_ = 1 / parameter.as_double(   );
           } else {
           RCLCPP_WARN(  
           get_logger(   ),  
           "The expected planner frequency parameter is %.4f Hz. The value should to be greater"
           " than 0.0 to turn on duration overrrun warning messages",   parameter.as_double(   ) );
           max_planner_duration_ = 0.0;
           }
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_planner
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
          RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_planner::PlannerServer )

navigation2/nav2_planner/test/test_dynamic_parameters.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_planner/planner_server.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      26  class PlannerShim : public nav2_planner::PlannerServer
          {
          public:
      29   PlannerShim(   )
           : nav2_planner::PlannerServer(  rclcpp::NodeOptions(   ) )
           {
           }
          
           // Since we cannot call configure/activate due to costmaps
           // requiring TF
      36   void setDynamicCallback(   )
           {
           auto node = shared_from_this(   );
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &PlannerShim::dynamicParamsShim,   this,   std::placeholders::_1 ) );
           }
          
           rcl_interfaces::msg::SetParametersResult
      45   dynamicParamsShim(  std::vector<rclcpp::Parameter> parameters )
           {
           rcl_interfaces::msg::SetParametersResult result;
           result.successful = true;
           dynamicParametersCallback(  parameters );
           return result;
           }
          };
          
      54  class RclCppFixture
          {
          public:
      57   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      58   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      62  TEST(  WPTest,   test_dynamic_parameters )
          {
           auto planner = std::make_shared<PlannerShim>(   );
           planner->setDynamicCallback(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           planner->get_node_base_interface(   ),   planner->get_node_topics_interface(   ),  
           planner->get_node_graph_interface(   ),  
           planner->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "expected_planner_frequency",   100.0 )} );
          
           rclcpp::spin_until_future_complete(  
           planner->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  planner->get_parameter(  "expected_planner_frequency" ).as_double(   ),   100.0 );
          
           // test edge case for = 0
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "expected_planner_frequency",   -1.0 )} );
          
           rclcpp::spin_until_future_complete(  
           planner->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  planner->get_parameter(  "expected_planner_frequency" ).as_double(   ),   -1.0 );
          }

navigation2/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

          // Copyright (  c ) 2020 Shrijit Singh
          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
          #define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <algorithm>
          #include <mutex>
          
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "nav2_core/controller.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_util/odometry_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          
          namespace nav2_regulated_pure_pursuit_controller
          {
          
          /**
           * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
           * @brief Regulated pure pursuit controller plugin
           */
      41  class RegulatedPurePursuitController : public nav2_core::Controller
          {
          public:
           /**
           * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
           */
           RegulatedPurePursuitController(   ) = default;
          
           /**
           * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
           */
           ~RegulatedPurePursuitController(   ) override = default;
          
           /**
           * @brief Configure controller state machine
           * @param parent WeakPtr to node
           * @param name Name of plugin
           * @param tf TF buffer
           * @param costmap_ros Costmap2DROS object of environment
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup controller state machine
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate controller state machine
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate controller state machine
           */
           void deactivate(   ) override;
          
           /**
           * @brief Compute the best command given the current pose and velocity,   with possible debug information
           *
           * Same as above computeVelocityCommands,   but with debug results.
           * If the results pointer is not null,   additional information about the twists
           * evaluated will be in results after the call.
           *
           * @param pose Current robot pose
           * @param velocity Current robot velocity
           * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
           * @return Best command
           */
           geometry_msgs::msg::TwistStamped computeVelocityCommands(  
           const geometry_msgs::msg::PoseStamped & pose,  
           const geometry_msgs::msg::Twist & velocity,  
           nav2_core::GoalChecker * /*goal_checker*/ ) override;
          
           /**
           * @brief nav2_core setPlan - Sets the global plan
           * @param path The global plan
           */
           void setPlan(  const nav_msgs::msg::Path & path ) override;
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
           void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) override;
          
          protected:
           /**
           * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint
           * Points ineligible to be selected as a lookahead point if they are any of the following:
           * - Outside the local_costmap (  collision avoidance cannot be assured )
           * @param pose pose to transform
           * @return Path in new frame
           */
           nav_msgs::msg::Path transformGlobalPlan(  
           const geometry_msgs::msg::PoseStamped & pose );
          
           /**
           * @brief Transform a pose to another frame.
           * @param frame Frame ID to transform to
           * @param in_pose Pose input to transform
           * @param out_pose transformed output
           * @return bool if successful
           */
           bool transformPose(  
           const std::string frame,  
           const geometry_msgs::msg::PoseStamped & in_pose,  
           geometry_msgs::msg::PoseStamped & out_pose ) const;
          
           /**
           * @brief Get lookahead distance
           * @param cmd the current speed to use to compute lookahead point
           * @return lookahead distance
           */
           double getLookAheadDistance(  const geometry_msgs::msg::Twist & );
          
           /**
           * @brief Creates a PointStamped message for visualization
           * @param carrot_pose Input carrot point as a PoseStamped
           * @return CarrotMsg a carrot point marker,   PointStamped
           */
           std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsg(  
           const geometry_msgs::msg::PoseStamped & carrot_pose );
          
           /**
           * @brief Whether robot should rotate to rough path heading
           * @param carrot_pose current lookahead point
           * @param angle_to_path Angle of robot output relatie to carrot marker
           * @return Whether should rotate to path heading
           */
           bool shouldRotateToPath(  
           const geometry_msgs::msg::PoseStamped & carrot_pose,   double & angle_to_path );
          
           /**
           * @brief Whether robot should rotate to final goal orientation
           * @param carrot_pose current lookahead point
           * @return Whether should rotate to goal heading
           */
           bool shouldRotateToGoalHeading(  const geometry_msgs::msg::PoseStamped & carrot_pose );
          
           /**
           * @brief Create a smooth and kinematically smoothed rotation command
           * @param linear_vel linear velocity
           * @param angular_vel angular velocity
           * @param angle_to_path Angle of robot output relatie to carrot marker
           * @param curr_speed the current robot speed
           */
           void rotateToHeading(  
           double & linear_vel,   double & angular_vel,  
           const double & angle_to_path,   const geometry_msgs::msg::Twist & curr_speed );
          
           /**
           * @brief Whether collision is imminent
           * @param robot_pose Pose of robot
           * @param carrot_pose Pose of carrot
           * @param linear_vel linear velocity to forward project
           * @param angular_vel angular velocity to forward project
           * @param carrot_dist Distance to the carrot for PP
           * @return Whether collision is imminent
           */
           bool isCollisionImminent(  
           const geometry_msgs::msg::PoseStamped &,  
           const double &,   const double &,  
           const double & );
          
           /**
           * @brief checks for collision at projected pose
           * @param x Pose of pose x
           * @param y Pose of pose y
           * @param theta orientation of Yaw
           * @return Whether in collision
           */
           bool inCollision(  
           const double & x,  
           const double & y,  
           const double & theta );
           /**
           * @brief Cost at a point
           * @param x Pose of pose x
           * @param y Pose of pose y
           * @return Cost of pose in costmap
           */
           double costAtPose(  const double & x,   const double & y );
          
           /**
           * @brief apply regulation constraints to the system
           * @param linear_vel robot command linear velocity input
           * @param dist_error error in the carrot distance and lookahead distance
           * @param lookahead_dist optimal lookahead distance
           * @param curvature curvature of path
           * @param speed Speed of robot
           * @param pose_cost cost at this pose
           */
           void applyConstraints(  
           const double & dist_error,   const double & lookahead_dist,  
           const double & curvature,   const geometry_msgs::msg::Twist & speed,  
           const double & pose_cost,   double & linear_vel,   double & sign );
          
           /**
           * @brief Find the intersection a circle and a line segment.
           * This assumes the circle is centered at the origin.
           * If no intersection is found,   a floating point error will occur.
           * @param p1 first endpoint of line segment
           * @param p2 second endpoint of line segment
           * @param r radius of circle
           * @return point of intersection
           */
           static geometry_msgs::msg::Point circleSegmentIntersection(  
           const geometry_msgs::msg::Point & p1,  
           const geometry_msgs::msg::Point & p2,  
           double r );
          
           /**
           * @brief Get lookahead point
           * @param lookahead_dist Optimal lookahead distance
           * @param path Current global path
           * @return Lookahead point
           */
           geometry_msgs::msg::PoseStamped getLookAheadPoint(  const double &,   const nav_msgs::msg::Path & );
          
           /**
           * @brief checks for the cusp position
           * @param pose Pose input to determine the cusp position
           * @return robot distance from the cusp
           */
           double findVelocitySignChange(  const nav_msgs::msg::Path & transformed_plan );
          
           /**
           * Get the greatest extent of the costmap in meters from the center.
           * @return max of distance from center in meters to edge of costmap
           */
           double getCostmapMaxExtent(   ) const;
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::string plugin_name_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
           nav2_costmap_2d::Costmap2D * costmap_;
           rclcpp::Logger logger_ {rclcpp::get_logger(  "RegulatedPurePursuitController" )};
           rclcpp::Clock::SharedPtr clock_;
          
           double desired_linear_vel_,   base_desired_linear_vel_;
           double lookahead_dist_;
           double rotate_to_heading_angular_vel_;
           double max_lookahead_dist_;
           double min_lookahead_dist_;
           double lookahead_time_;
           bool use_velocity_scaled_lookahead_dist_;
           tf2::Duration transform_tolerance_;
           double min_approach_linear_velocity_;
           double control_duration_;
           double max_allowed_time_to_collision_up_to_carrot_;
           bool use_regulated_linear_velocity_scaling_;
           bool use_cost_regulated_linear_velocity_scaling_;
           double cost_scaling_dist_;
           double cost_scaling_gain_;
           double inflation_cost_scaling_factor_;
           double regulated_linear_scaling_min_radius_;
           double regulated_linear_scaling_min_speed_;
           bool use_rotate_to_heading_;
           double max_angular_accel_;
           double rotate_to_heading_min_angle_;
           double goal_dist_tol_;
           bool allow_reversing_;
           double max_robot_pose_search_dist_;
           bool use_interpolation_;
          
           nav_msgs::msg::Path global_plan_;
     302   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
     303   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
           carrot_pub_;
     305   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
     306   std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
           collision_checker_;
          
           // Dynamic parameters handler
           std::mutex mutex_;
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_regulated_pure_pursuit_controller
          
          #endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_

navigation2/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

       1  // Copyright (  c ) 2020 Shrijit Singh
          // Copyright (  c ) 2020 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <algorithm>
          #include <string>
          #include <limits>
          #include <memory>
          #include <vector>
          #include <utility>
          
          #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
          #include "nav2_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          
          using std::hypot;
          using std::min;
          using std::max;
          using std::abs;
          using nav2_util::declare_parameter_if_not_declared;
          using nav2_util::geometry_utils::euclidean_distance;
          using namespace nav2_costmap_2d; // NOLINT
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_regulated_pure_pursuit_controller
          {
          
      41  void RegulatedPurePursuitController::configure(  
      42   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      43   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      44   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           auto node = parent.lock(   );
           node_ = parent;
           if (  !node ) {
           throw nav2_core::PlannerException(  "Unable to lock node!" );
           }
          
           costmap_ros_ = costmap_ros;
           costmap_ = costmap_ros_->getCostmap(   );
           tf_ = tf;
           plugin_name_ = name;
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
          
           double transform_tolerance = 0.1;
           double control_frequency = 20.0;
           goal_dist_tol_ = 0.25; // reasonable default before first update
          
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".desired_linear_vel",   rclcpp::ParameterValue(  0.5 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".lookahead_dist",   rclcpp::ParameterValue(  0.6 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".min_lookahead_dist",   rclcpp::ParameterValue(  0.3 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".max_lookahead_dist",   rclcpp::ParameterValue(  0.9 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".lookahead_time",   rclcpp::ParameterValue(  1.5 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".rotate_to_heading_angular_vel",   rclcpp::ParameterValue(  1.8 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".transform_tolerance",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".use_velocity_scaled_lookahead_dist",  
           rclcpp::ParameterValue(  false ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".min_approach_linear_velocity",   rclcpp::ParameterValue(  0.05 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",  
           rclcpp::ParameterValue(  1.0 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".use_regulated_linear_velocity_scaling",   rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",  
           rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".cost_scaling_dist",   rclcpp::ParameterValue(  0.6 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".cost_scaling_gain",   rclcpp::ParameterValue(  1.0 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".inflation_cost_scaling_factor",   rclcpp::ParameterValue(  3.0 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".regulated_linear_scaling_min_radius",   rclcpp::ParameterValue(  0.90 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".regulated_linear_scaling_min_speed",   rclcpp::ParameterValue(  0.25 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".use_rotate_to_heading",   rclcpp::ParameterValue(  true ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".rotate_to_heading_min_angle",   rclcpp::ParameterValue(  0.785 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".max_angular_accel",   rclcpp::ParameterValue(  3.2 ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".allow_reversing",   rclcpp::ParameterValue(  false ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".max_robot_pose_search_dist",  
           rclcpp::ParameterValue(  getCostmapMaxExtent(   ) ) );
           declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".use_interpolation",  
           rclcpp::ParameterValue(  true ) );
          
           node->get_parameter(  plugin_name_ + ".desired_linear_vel",   desired_linear_vel_ );
           base_desired_linear_vel_ = desired_linear_vel_;
           node->get_parameter(  plugin_name_ + ".lookahead_dist",   lookahead_dist_ );
           node->get_parameter(  plugin_name_ + ".min_lookahead_dist",   min_lookahead_dist_ );
           node->get_parameter(  plugin_name_ + ".max_lookahead_dist",   max_lookahead_dist_ );
           node->get_parameter(  plugin_name_ + ".lookahead_time",   lookahead_time_ );
           node->get_parameter(  
           plugin_name_ + ".rotate_to_heading_angular_vel",  
           rotate_to_heading_angular_vel_ );
           node->get_parameter(  plugin_name_ + ".transform_tolerance",   transform_tolerance );
           node->get_parameter(  
           plugin_name_ + ".use_velocity_scaled_lookahead_dist",  
           use_velocity_scaled_lookahead_dist_ );
           node->get_parameter(  
           plugin_name_ + ".min_approach_linear_velocity",  
           min_approach_linear_velocity_ );
           node->get_parameter(  
           plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",  
           max_allowed_time_to_collision_up_to_carrot_ );
           node->get_parameter(  
           plugin_name_ + ".use_regulated_linear_velocity_scaling",  
           use_regulated_linear_velocity_scaling_ );
           node->get_parameter(  
           plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",  
           use_cost_regulated_linear_velocity_scaling_ );
           node->get_parameter(  plugin_name_ + ".cost_scaling_dist",   cost_scaling_dist_ );
           node->get_parameter(  plugin_name_ + ".cost_scaling_gain",   cost_scaling_gain_ );
           node->get_parameter(  
           plugin_name_ + ".inflation_cost_scaling_factor",  
           inflation_cost_scaling_factor_ );
           node->get_parameter(  
           plugin_name_ + ".regulated_linear_scaling_min_radius",  
           regulated_linear_scaling_min_radius_ );
           node->get_parameter(  
           plugin_name_ + ".regulated_linear_scaling_min_speed",  
           regulated_linear_scaling_min_speed_ );
           node->get_parameter(  plugin_name_ + ".use_rotate_to_heading",   use_rotate_to_heading_ );
           node->get_parameter(  plugin_name_ + ".rotate_to_heading_min_angle",   rotate_to_heading_min_angle_ );
           node->get_parameter(  plugin_name_ + ".max_angular_accel",   max_angular_accel_ );
           node->get_parameter(  plugin_name_ + ".allow_reversing",   allow_reversing_ );
           node->get_parameter(  "controller_frequency",   control_frequency );
           node->get_parameter(  
           plugin_name_ + ".max_robot_pose_search_dist",  
           max_robot_pose_search_dist_ );
           node->get_parameter(  
           plugin_name_ + ".use_interpolation",  
           use_interpolation_ );
          
           transform_tolerance_ = tf2::durationFromSec(  transform_tolerance );
           control_duration_ = 1.0 / control_frequency;
          
           if (  inflation_cost_scaling_factor_ <= 0.0 ) {
           RCLCPP_WARN(  
           logger_,   "The value inflation_cost_scaling_factor is incorrectly set,   "
           "it should be >0. Disabling cost regulated linear velocity scaling." );
           use_cost_regulated_linear_velocity_scaling_ = false;
           }
          
           /** Possible to drive in reverse direction if and only if
           "use_rotate_to_heading" parameter is set to false **/
          
           if (  use_rotate_to_heading_ && allow_reversing_ ) {
           RCLCPP_WARN(  
           logger_,   "Disabling reversing. Both use_rotate_to_heading and allow_reversing "
           "parameter cannot be set to true. By default setting use_rotate_to_heading true" );
           allow_reversing_ = false;
           }
          
           global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(  "received_global_plan",   1 );
           carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(  "lookahead_point",   1 );
           carrot_arc_pub_ = node->create_publisher<nav_msgs::msg::Path>(  "lookahead_collision_arc",   1 );
          
           // initialize collision checker and set costmap
           collision_checker_ = std::make_unique<nav2_costmap_2d::
           FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(  costmap_ );
           collision_checker_->setCostmap(  costmap_ );
          }
          
     193  void RegulatedPurePursuitController::cleanup(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Cleaning up controller: %s of type"
           " regulated_pure_pursuit_controller::RegulatedPurePursuitController",  
           plugin_name_.c_str(   ) );
           global_path_pub_.reset(   );
           carrot_pub_.reset(   );
           carrot_arc_pub_.reset(   );
          }
          
     205  void RegulatedPurePursuitController::activate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Activating controller: %s of type "
           "regulated_pure_pursuit_controller::RegulatedPurePursuitController",  
           plugin_name_.c_str(   ) );
           global_path_pub_->on_activate(   );
           carrot_pub_->on_activate(   );
           carrot_arc_pub_->on_activate(   );
           // Add callback for dynamic parameters
           auto node = node_.lock(   );
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &RegulatedPurePursuitController::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
          }
          
     223  void RegulatedPurePursuitController::deactivate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Deactivating controller: %s of type "
           "regulated_pure_pursuit_controller::RegulatedPurePursuitController",  
           plugin_name_.c_str(   ) );
           global_path_pub_->on_deactivate(   );
           carrot_pub_->on_deactivate(   );
           carrot_arc_pub_->on_deactivate(   );
           dyn_params_handler_.reset(   );
          }
          
     236  std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(  
     237   const geometry_msgs::msg::PoseStamped & carrot_pose )
          {
           auto carrot_msg = std::make_unique<geometry_msgs::msg::PointStamped>(   );
           carrot_msg->header = carrot_pose.header;
           carrot_msg->point.x = carrot_pose.pose.position.x;
           carrot_msg->point.y = carrot_pose.pose.position.y;
           carrot_msg->point.z = 0.01; // publish right over map to stand out
           return carrot_msg;
          }
          
     247  double RegulatedPurePursuitController::getLookAheadDistance(  
     248   const geometry_msgs::msg::Twist & speed )
          {
           // If using velocity-scaled look ahead distances,   find and clamp the dist
           // Else,   use the static look ahead distance
           double lookahead_dist = lookahead_dist_;
           if (  use_velocity_scaled_lookahead_dist_ ) {
           lookahead_dist = fabs(  speed.linear.x ) * lookahead_time_;
           lookahead_dist = std::clamp(  lookahead_dist,   min_lookahead_dist_,   max_lookahead_dist_ );
           }
          
           return lookahead_dist;
          }
          
     261  geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(  
     262   const geometry_msgs::msg::PoseStamped & pose,  
     263   const geometry_msgs::msg::Twist & speed,  
     264   nav2_core::GoalChecker * goal_checker )
          {
           std::lock_guard<std::mutex> lock_reinit(  mutex_ );
          
           // Update for the current goal checker's state
           geometry_msgs::msg::Pose pose_tolerance;
           geometry_msgs::msg::Twist vel_tolerance;
           if (  !goal_checker->getTolerances(  pose_tolerance,   vel_tolerance ) ) {
           RCLCPP_WARN(  logger_,   "Unable to retrieve goal checker's tolerances!" );
           } else {
           goal_dist_tol_ = pose_tolerance.position.x;
           }
          
           // Transform path to robot base frame
           auto transformed_plan = transformGlobalPlan(  pose );
          
           // Find look ahead distance and point on path and publish
           double lookahead_dist = getLookAheadDistance(  speed );
          
           // Check for reverse driving
           if (  allow_reversing_ ) {
           // Cusp check
           double dist_to_cusp = findVelocitySignChange(  transformed_plan );
          
           // if the lookahead distance is further than the cusp,   use the cusp distance instead
           if (  dist_to_cusp < lookahead_dist ) {
           lookahead_dist = dist_to_cusp;
           }
           }
          
           auto carrot_pose = getLookAheadPoint(  lookahead_dist,   transformed_plan );
           carrot_pub_->publish(  createCarrotMsg(  carrot_pose ) );
          
           double linear_vel,   angular_vel;
          
           // Find distance^2 to look ahead point (  carrot ) in robot base frame
           // This is the chord length of the circle
           const double carrot_dist2 =
           (  carrot_pose.pose.position.x * carrot_pose.pose.position.x ) +
           (  carrot_pose.pose.position.y * carrot_pose.pose.position.y );
          
           // Find curvature of circle (  k = 1 / R )
           double curvature = 0.0;
           if (  carrot_dist2 > 0.001 ) {
           curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2;
           }
          
           // Setting the velocity direction
           double sign = 1.0;
           if (  allow_reversing_ ) {
           sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
           }
          
           linear_vel = desired_linear_vel_;
          
           // Make sure we're in compliance with basic constraints
           double angle_to_heading;
           if (  shouldRotateToGoalHeading(  carrot_pose ) ) {
           double angle_to_goal = tf2::getYaw(  transformed_plan.poses.back(   ).pose.orientation );
           rotateToHeading(  linear_vel,   angular_vel,   angle_to_goal,   speed );
           } else if (  shouldRotateToPath(  carrot_pose,   angle_to_heading ) ) {
           rotateToHeading(  linear_vel,   angular_vel,   angle_to_heading,   speed );
           } else {
           applyConstraints(  
           fabs(  lookahead_dist - sqrt(  carrot_dist2 ) ),  
           lookahead_dist,   curvature,   speed,  
           costAtPose(  pose.pose.position.x,   pose.pose.position.y ),   linear_vel,   sign );
          
           // Apply curvature to angular velocity after constraining linear velocity
           angular_vel = sign * linear_vel * curvature;
           }
          
           // Collision checking on this velocity heading
           const double & carrot_dist = hypot(  carrot_pose.pose.position.x,   carrot_pose.pose.position.y );
           if (  isCollisionImminent(  pose,   linear_vel,   angular_vel,   carrot_dist ) ) {
           throw nav2_core::PlannerException(  "RegulatedPurePursuitController detected collision ahead!" );
           }
          
           // populate and return message
           geometry_msgs::msg::TwistStamped cmd_vel;
           cmd_vel.header = pose.header;
           cmd_vel.twist.linear.x = linear_vel;
           cmd_vel.twist.angular.z = angular_vel;
           return cmd_vel;
          }
          
     350  bool RegulatedPurePursuitController::shouldRotateToPath(  
     351   const geometry_msgs::msg::PoseStamped & carrot_pose,   double & angle_to_path )
          {
           // Whether we should rotate robot to rough path heading
           angle_to_path = atan2(  carrot_pose.pose.position.y,   carrot_pose.pose.position.x );
           return use_rotate_to_heading_ && fabs(  angle_to_path ) > rotate_to_heading_min_angle_;
          }
          
     358  bool RegulatedPurePursuitController::shouldRotateToGoalHeading(  
     359   const geometry_msgs::msg::PoseStamped & carrot_pose )
          {
           // Whether we should rotate robot to goal heading
           double dist_to_goal = std::hypot(  carrot_pose.pose.position.x,   carrot_pose.pose.position.y );
           return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_;
          }
          
     366  void RegulatedPurePursuitController::rotateToHeading(  
           double & linear_vel,   double & angular_vel,  
     368   const double & angle_to_path,   const geometry_msgs::msg::Twist & curr_speed )
          {
           // Rotate in place using max angular velocity / acceleration possible
           linear_vel = 0.0;
           const double sign = angle_to_path > 0.0 ? 1.0 : -1.0;
           angular_vel = sign * rotate_to_heading_angular_vel_;
          
           const double & dt = control_duration_;
           const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt;
           const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt;
           angular_vel = std::clamp(  angular_vel,   min_feasible_angular_speed,   max_feasible_angular_speed );
          }
          
     381  geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(  
     382   const geometry_msgs::msg::Point & p1,  
     383   const geometry_msgs::msg::Point & p2,  
           double r )
          {
           // Formula for intersection of a line with a circle centered at the origin,  
           // modified to always return the point that is on the segment between the two points.
           // https://mathworld.wolfram.com/Circle-LineIntersection.html
           // This works because the poses are transformed into the robot frame.
           // This can be derived from solving the system of equations of a line and a circle
           // which results in something that is just a reformulation of the quadratic formula.
           // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
           // https://www.desmos.com/calculator/td5cwbuocd
           double x1 = p1.x;
           double x2 = p2.x;
           double y1 = p1.y;
           double y2 = p2.y;
          
           double dx = x2 - x1;
           double dy = y2 - y1;
           double dr2 = dx * dx + dy * dy;
           double D = x1 * y2 - x2 * y1;
          
           // Augmentation to only return point within segment
           double d1 = x1 * x1 + y1 * y1;
           double d2 = x2 * x2 + y2 * y2;
           double dd = d2 - d1;
          
           geometry_msgs::msg::Point p;
           double sqrt_term = std::sqrt(  r * r * dr2 - D * D );
           p.x = (  D * dy + std::copysign(  1.0,   dd ) * dx * sqrt_term ) / dr2;
           p.y = (  -D * dx + std::copysign(  1.0,   dd ) * dy * sqrt_term ) / dr2;
           return p;
          }
          
     416  geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(  
           const double & lookahead_dist,  
     418   const nav_msgs::msg::Path & transformed_plan )
          {
           // Find the first pose which is at a distance greater than the lookahead distance
           auto goal_pose_it = std::find_if(  
           transformed_plan.poses.begin(   ),   transformed_plan.poses.end(   ),   [&](  const auto & ps ) {
           return hypot(  ps.pose.position.x,   ps.pose.position.y ) >= lookahead_dist;
           } );
          
           // If the no pose is not far enough,   take the last pose
           if (  goal_pose_it == transformed_plan.poses.end(   ) ) {
           goal_pose_it = std::prev(  transformed_plan.poses.end(   ) );
           } else if (  use_interpolation_ && goal_pose_it != transformed_plan.poses.begin(   ) ) {
           // Find the point on the line segment between the two poses
           // that is exactly the lookahead distance away from the robot pose (  the origin )
           // This can be found with a closed form for the intersection of a segment and a circle
           // Because of the way we did the std::find_if,   prev_pose is guaranteed to be inside the circle,  
           // and goal_pose is guaranteed to be outside the circle.
           auto prev_pose_it = std::prev(  goal_pose_it );
           auto point = circleSegmentIntersection(  
           prev_pose_it->pose.position,  
           goal_pose_it->pose.position,   lookahead_dist );
           geometry_msgs::msg::PoseStamped pose;
           pose.header.frame_id = prev_pose_it->header.frame_id;
           pose.header.stamp = goal_pose_it->header.stamp;
           pose.pose.position = point;
           return pose;
           }
          
           return *goal_pose_it;
          }
          
     449  bool RegulatedPurePursuitController::isCollisionImminent(  
     450   const geometry_msgs::msg::PoseStamped & robot_pose,  
           const double & linear_vel,   const double & angular_vel,  
           const double & carrot_dist )
          {
           // Note(  stevemacenski ): This may be a bit unusual,   but the robot_pose is in
           // odom frame and the carrot_pose is in robot base frame.
          
           // check current point is OK
           if (  inCollision(  
           robot_pose.pose.position.x,   robot_pose.pose.position.y,  
           tf2::getYaw(  robot_pose.pose.orientation ) ) )
           {
           return true;
           }
          
           // visualization messages
           nav_msgs::msg::Path arc_pts_msg;
           arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(   );
           arc_pts_msg.header.stamp = robot_pose.header.stamp;
           geometry_msgs::msg::PoseStamped pose_msg;
           pose_msg.header.frame_id = arc_pts_msg.header.frame_id;
           pose_msg.header.stamp = arc_pts_msg.header.stamp;
          
           double projection_time = 0.0;
           if (  fabs(  linear_vel ) < 0.01 && fabs(  angular_vel ) > 0.01 ) {
           // rotating to heading at goal or toward path
           // Equation finds the angular distance required for the largest
           // part of the robot radius to move to another costmap cell:
           // theta_min = 2.0 * sin (  (  res/2 ) / r_max )
           // via isosceles triangle r_max-r_max-resolution,  
           // dividing by angular_velocity gives us a timestep.
           double max_radius = costmap_ros_->getLayeredCostmap(   )->getCircumscribedRadius(   );
           projection_time =
           2.0 * sin(  (  costmap_->getResolution(   ) / 2 ) / max_radius ) / fabs(  angular_vel );
           } else {
           // Normal path tracking
           projection_time = costmap_->getResolution(   ) / fabs(  linear_vel );
           }
          
           const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position;
           geometry_msgs::msg::Pose2D curr_pose;
           curr_pose.x = robot_pose.pose.position.x;
           curr_pose.y = robot_pose.pose.position.y;
           curr_pose.theta = tf2::getYaw(  robot_pose.pose.orientation );
          
           // only forward simulate within time requested
           int i = 1;
           while (  i * projection_time < max_allowed_time_to_collision_up_to_carrot_ ) {
           i++;
          
           // apply velocity at curr_pose over distance
           curr_pose.x += projection_time * (  linear_vel * cos(  curr_pose.theta ) );
           curr_pose.y += projection_time * (  linear_vel * sin(  curr_pose.theta ) );
           curr_pose.theta += projection_time * angular_vel;
          
           // check if past carrot pose,   where no longer a thoughtfully valid command
           if (  hypot(  curr_pose.x - robot_xy.x,   curr_pose.y - robot_xy.y ) > carrot_dist ) {
           break;
           }
          
           // store it for visualization
           pose_msg.pose.position.x = curr_pose.x;
           pose_msg.pose.position.y = curr_pose.y;
           pose_msg.pose.position.z = 0.01;
           arc_pts_msg.poses.push_back(  pose_msg );
          
           // check for collision at the projected pose
           if (  inCollision(  curr_pose.x,   curr_pose.y,   curr_pose.theta ) ) {
           carrot_arc_pub_->publish(  arc_pts_msg );
           return true;
           }
           }
          
           carrot_arc_pub_->publish(  arc_pts_msg );
          
           return false;
          }
          
     528  bool RegulatedPurePursuitController::inCollision(  
           const double & x,  
           const double & y,  
           const double & theta )
          {
           unsigned int mx,   my;
          
           if (  !costmap_->worldToMap(  x,   y,   mx,   my ) ) {
           RCLCPP_WARN_THROTTLE(  
           logger_,   *(  clock_ ),   30000,  
           "The dimensions of the costmap is too small to successfully check for "
           "collisions as far ahead as requested. Proceed at your own risk,   slow the robot,   or "
           "increase your costmap size." );
           return false;
           }
          
           double footprint_cost = collision_checker_->footprintCostAtPose(  
           x,   y,   theta,   costmap_ros_->getRobotFootprint(   ) );
           if (  footprint_cost == static_cast<double>(  NO_INFORMATION ) &&
           costmap_ros_->getLayeredCostmap(   )->isTrackingUnknown(   ) )
           {
           return false;
           }
          
           // if occupied or unknown and not to traverse unknown space
           return footprint_cost >= static_cast<double>(  LETHAL_OBSTACLE );
          }
          
     556  double RegulatedPurePursuitController::costAtPose(  const double & x,   const double & y )
          {
           unsigned int mx,   my;
          
           if (  !costmap_->worldToMap(  x,   y,   mx,   my ) ) {
           RCLCPP_FATAL(  
           logger_,  
           "The dimensions of the costmap is too small to fully include your robot's footprint,   "
           "thusly the robot cannot proceed further" );
           throw nav2_core::PlannerException(  
           "RegulatedPurePursuitController: Dimensions of the costmap are too small "
           "to encapsulate the robot footprint at current speeds!" );
           }
          
           unsigned char cost = costmap_->getCost(  mx,   my );
           return static_cast<double>(  cost );
          }
          
     574  void RegulatedPurePursuitController::applyConstraints(  
           const double & dist_error,   const double & lookahead_dist,  
     576   const double & curvature,   const geometry_msgs::msg::Twist & /*curr_speed*/,  
           const double & pose_cost,   double & linear_vel,   double & sign )
          {
           double curvature_vel = linear_vel;
           double cost_vel = linear_vel;
           double approach_vel = linear_vel;
          
           // limit the linear velocity by curvature
           const double radius = fabs(  1.0 / curvature );
           const double & min_rad = regulated_linear_scaling_min_radius_;
           if (  use_regulated_linear_velocity_scaling_ && radius < min_rad ) {
           curvature_vel *= 1.0 - (  fabs(  radius - min_rad ) / min_rad );
           }
          
           // limit the linear velocity by proximity to obstacles
           if (  use_cost_regulated_linear_velocity_scaling_ &&
           pose_cost != static_cast<double>(  NO_INFORMATION ) &&
           pose_cost != static_cast<double>(  FREE_SPACE ) )
           {
           const double inscribed_radius = costmap_ros_->getLayeredCostmap(   )->getInscribedRadius(   );
           const double min_distance_to_obstacle = (  -1.0 / inflation_cost_scaling_factor_ ) *
           std::log(  pose_cost / (  INSCRIBED_INFLATED_OBSTACLE - 1 ) ) + inscribed_radius;
          
           if (  min_distance_to_obstacle < cost_scaling_dist_ ) {
           cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
           }
           }
          
           // Use the lowest of the 2 constraint heuristics,   but above the minimum translational speed
           linear_vel = std::min(  cost_vel,   curvature_vel );
           linear_vel = std::max(  linear_vel,   regulated_linear_scaling_min_speed_ );
          
           // if the actual lookahead distance is shorter than requested,   that means we're at the
           // end of the path. We'll scale linear velocity by error to slow to a smooth stop.
           // This expression is eq. to (  1 ) holding time to goal,   t,   constant using the theoretical
           // lookahead distance and proposed velocity and (  2 ) using t with the actual lookahead
           // distance to scale the velocity (  e.g. t = lookahead / velocity,   v = carrot / t ).
           if (  dist_error > 2.0 * costmap_->getResolution(   ) ) {
           double velocity_scaling = 1.0 - (  dist_error / lookahead_dist );
           double unbounded_vel = approach_vel * velocity_scaling;
           if (  unbounded_vel < min_approach_linear_velocity_ ) {
           approach_vel = min_approach_linear_velocity_;
           } else {
           approach_vel *= velocity_scaling;
           }
          
           // Use the lowest velocity between approach and other constraints,   if all overlapping
           linear_vel = std::min(  linear_vel,   approach_vel );
           }
          
           // Limit linear velocities to be valid
           linear_vel = std::clamp(  fabs(  linear_vel ),   0.0,   desired_linear_vel_ );
           linear_vel = sign * linear_vel;
          }
          
     631  void RegulatedPurePursuitController::setPlan(  const nav_msgs::msg::Path & path )
          {
           global_plan_ = path;
          }
          
     636  void RegulatedPurePursuitController::setSpeedLimit(  
           const double & speed_limit,  
     638   const bool & percentage )
          {
           if (  speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT ) {
           // Restore default value
           desired_linear_vel_ = base_desired_linear_vel_;
           } else {
           if (  percentage ) {
           // Speed limit is expressed in % from maximum speed of robot
           desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0;
           } else {
           // Speed limit is expressed in absolute value
           desired_linear_vel_ = speed_limit;
           }
           }
          }
          
     654  nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(  
     655   const geometry_msgs::msg::PoseStamped & pose )
          {
           if (  global_plan_.poses.empty(   ) ) {
           throw nav2_core::PlannerException(  "Received plan with zero length" );
           }
          
           // let's get the pose of the robot in the frame of the plan
           geometry_msgs::msg::PoseStamped robot_pose;
           if (  !transformPose(  global_plan_.header.frame_id,   pose,   robot_pose ) ) {
           throw nav2_core::PlannerException(  "Unable to transform robot pose into global plan's frame" );
           }
          
           // We'll discard points on the plan that are outside the local costmap
           double max_costmap_extent = getCostmapMaxExtent(   );
          
           auto closest_pose_upper_bound =
           nav2_util::geometry_utils::first_after_integrated_distance(  
           global_plan_.poses.begin(   ),   global_plan_.poses.end(   ),   max_robot_pose_search_dist_ );
          
           // First find the closest pose on the path to the robot
           // bounded by when the path turns around (  if it does ) so we don't get a pose from a later
           // portion of the path
           auto transformation_begin =
           nav2_util::geometry_utils::min_by(  
           global_plan_.poses.begin(   ),   closest_pose_upper_bound,  
           [&robot_pose](  const geometry_msgs::msg::PoseStamped & ps ) {
           return euclidean_distance(  robot_pose,   ps );
           } );
          
           // Find points up to max_transform_dist so we only transform them.
           auto transformation_end = std::find_if(  
           transformation_begin,   global_plan_.poses.end(   ),  
           [&](  const auto & pose ) {
           return euclidean_distance(  pose,   robot_pose ) > max_costmap_extent;
           } );
          
           // Lambda to transform a PoseStamped from global frame to local
           auto transformGlobalPoseToLocal = [&](  const auto & global_plan_pose ) {
           geometry_msgs::msg::PoseStamped stamped_pose,   transformed_pose;
           stamped_pose.header.frame_id = global_plan_.header.frame_id;
           stamped_pose.header.stamp = robot_pose.header.stamp;
           stamped_pose.pose = global_plan_pose.pose;
           transformPose(  costmap_ros_->getBaseFrameID(   ),   stamped_pose,   transformed_pose );
           transformed_pose.pose.position.z = 0.0;
           return transformed_pose;
           };
          
           // Transform the near part of the global plan into the robot's frame of reference.
           nav_msgs::msg::Path transformed_plan;
           std::transform(  
           transformation_begin,   transformation_end,  
           std::back_inserter(  transformed_plan.poses ),  
           transformGlobalPoseToLocal );
           transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(   );
           transformed_plan.header.stamp = robot_pose.header.stamp;
          
           // Remove the portion of the global plan that we've already passed so we don't
           // process it on the next iteration (  this is called path pruning )
           global_plan_.poses.erase(  begin(  global_plan_.poses ),   transformation_begin );
           global_path_pub_->publish(  transformed_plan );
          
           if (  transformed_plan.poses.empty(   ) ) {
           throw nav2_core::PlannerException(  "Resulting plan has 0 poses in it." );
           }
          
           return transformed_plan;
          }
          
     723  double RegulatedPurePursuitController::findVelocitySignChange(  
     724   const nav_msgs::msg::Path & transformed_plan )
          {
           // Iterating through the transformed global path to determine the position of the cusp
           for (  unsigned int pose_id = 1; pose_id < transformed_plan.poses.size(   ) - 1; ++pose_id ) {
           // We have two vectors for the dot product OA and AB. Determining the vectors.
           double oa_x = transformed_plan.poses[pose_id].pose.position.x -
           transformed_plan.poses[pose_id - 1].pose.position.x;
           double oa_y = transformed_plan.poses[pose_id].pose.position.y -
           transformed_plan.poses[pose_id - 1].pose.position.y;
           double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
           transformed_plan.poses[pose_id].pose.position.x;
           double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
           transformed_plan.poses[pose_id].pose.position.y;
          
           /* Checking for the existance of cusp,   in the path,   using the dot product
           and determine it's distance from the robot. If there is no cusp in the path,  
           then just determine the distance to the goal location. */
           if (   (  oa_x * ab_x ) + (  oa_y * ab_y ) < 0.0 ) {
           // returning the distance if there is a cusp
           // The transformed path is in the robots frame,   so robot is at the origin
           return hypot(  
           transformed_plan.poses[pose_id].pose.position.x,  
           transformed_plan.poses[pose_id].pose.position.y );
           }
           }
          
           return std::numeric_limits<double>::max(   );
          }
          
     753  bool RegulatedPurePursuitController::transformPose(  
     754   const std::string frame,  
     755   const geometry_msgs::msg::PoseStamped & in_pose,  
     756   geometry_msgs::msg::PoseStamped & out_pose ) const
          {
           if (  in_pose.header.frame_id == frame ) {
           out_pose = in_pose;
           return true;
           }
          
           try {
           tf_->transform(  in_pose,   out_pose,   frame,   transform_tolerance_ );
           out_pose.header.frame_id = frame;
           return true;
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  logger_,   "Exception in transformPose: %s",   ex.what(   ) );
           }
           return false;
          }
          
     773  double RegulatedPurePursuitController::getCostmapMaxExtent(   ) const
          {
           const double max_costmap_dim_meters = std::max(  
           costmap_->getSizeInMetersX(   ),   costmap_->getSizeInMetersX(   ) );
           return max_costmap_dim_meters / 2.0;
          }
          
          
          rcl_interfaces::msg::SetParametersResult
     782  RegulatedPurePursuitController::dynamicParametersCallback(  
     783   std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           std::lock_guard<std::mutex> lock_reinit(  mutex_ );
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".inflation_cost_scaling_factor" ) {
           if (  parameter.as_double(   ) <= 0.0 ) {
           RCLCPP_WARN(  
           logger_,   "The value inflation_cost_scaling_factor is incorrectly set,   "
           "it should be >0. Ignoring parameter update." );
           continue;
           }
           inflation_cost_scaling_factor_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".desired_linear_vel" ) {
           desired_linear_vel_ = parameter.as_double(   );
           base_desired_linear_vel_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".lookahead_dist" ) {
           lookahead_dist_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".max_lookahead_dist" ) {
           max_lookahead_dist_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".min_lookahead_dist" ) {
           min_lookahead_dist_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".lookahead_time" ) {
           lookahead_time_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".rotate_to_heading_angular_vel" ) {
           rotate_to_heading_angular_vel_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".min_approach_linear_velocity" ) {
           min_approach_linear_velocity_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot" ) {
           max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".cost_scaling_dist" ) {
           cost_scaling_dist_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".cost_scaling_gain" ) {
           cost_scaling_gain_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".regulated_linear_scaling_min_radius" ) {
           regulated_linear_scaling_min_radius_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".transform_tolerance" ) {
           double transform_tolerance = parameter.as_double(   );
           transform_tolerance_ = tf2::durationFromSec(  transform_tolerance );
           } else if (  name == plugin_name_ + ".regulated_linear_scaling_min_speed" ) {
           regulated_linear_scaling_min_speed_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".max_angular_accel" ) {
           max_angular_accel_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".rotate_to_heading_min_angle" ) {
           rotate_to_heading_min_angle_ = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == plugin_name_ + ".use_velocity_scaled_lookahead_dist" ) {
           use_velocity_scaled_lookahead_dist_ = parameter.as_bool(   );
           } else if (  name == plugin_name_ + ".use_regulated_linear_velocity_scaling" ) {
           use_regulated_linear_velocity_scaling_ = parameter.as_bool(   );
           } else if (  name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling" ) {
           use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(   );
           } else if (  name == plugin_name_ + ".use_rotate_to_heading" ) {
           if (  parameter.as_bool(   ) && allow_reversing_ ) {
           RCLCPP_WARN(  
           logger_,   "Both use_rotate_to_heading and allow_reversing "
           "parameter cannot be set to true. Rejecting parameter update." );
           continue;
           }
           use_rotate_to_heading_ = parameter.as_bool(   );
           } else if (  name == plugin_name_ + ".allow_reversing" ) {
           if (  use_rotate_to_heading_ && parameter.as_bool(   ) ) {
           RCLCPP_WARN(  
           logger_,   "Both use_rotate_to_heading and allow_reversing "
           "parameter cannot be set to true. Rejecting parameter update." );
           continue;
           }
           allow_reversing_ = parameter.as_bool(   );
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_regulated_pure_pursuit_controller
          
          // Register this controller as a nav2_core plugin
     868  PLUGINLIB_EXPORT_CLASS(  
           nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController,  
           nav2_core::Controller )

navigation2/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp

       1  // Copyright (  c ) 2022 Adam Aposhian
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "path_utils.hpp"
          
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          
          namespace path_utils
          {
          
      24  void append_transform_to_path(  
           nav_msgs::msg::Path & path,  
           tf2::Transform & relative_transform )
          {
           // Add a new empty pose
           path.poses.emplace_back(   );
           // Get the previous,   last pose (  after the emplace_back so the reference isn't invalidated )
           auto & previous_pose = *(  path.poses.end(   ) - 2 );
           auto & new_pose = path.poses.back(   );
          
           // get map_transform of previous_pose
           tf2::Transform map_transform;
           tf2::fromMsg(  previous_pose.pose,   map_transform );
          
           tf2::Transform full_transform;
           full_transform.mult(  map_transform,   relative_transform );
          
           tf2::toMsg(  full_transform,   new_pose.pose );
           new_pose.header.frame_id = previous_pose.header.frame_id;
          }
          
      45  void Straight::append(  nav_msgs::msg::Path & path,   double spacing ) const
          {
           auto num_points = std::floor(  length_ / spacing );
           path.poses.reserve(  path.poses.size(   ) + num_points );
           tf2::Transform translation(  tf2::Quaternion::getIdentity(   ),   tf2::Vector3(  spacing,   0.0,   0.0 ) );
           for (  size_t i = 1; i <= num_points; ++i ) {
           append_transform_to_path(  path,   translation );
           }
          }
          
      55  double chord_length(  double radius,   double radians )
          {
           return 2 * radius * sin(  radians / 2 );
          }
          
      60  void Arc::append(  nav_msgs::msg::Path & path,   double spacing ) const
          {
           double length = radius_ * std::abs(  radians_ );
           size_t num_points = std::floor(  length / spacing );
           double radians_per_step = radians_ / num_points;
           tf2::Transform transform(  
           tf2::Quaternion(  tf2::Vector3(  0.0,   0.0,   1.0 ),   radians_per_step ),  
           tf2::Vector3(  chord_length(  radius_,   std::abs(  radians_per_step ) ),   0.0,   0.0 ) );
           path.poses.reserve(  path.poses.size(   ) + num_points );
           for (  size_t i = 0; i < num_points; ++i ) {
           append_transform_to_path(  path,   transform );
           }
          }
          
      74  nav_msgs::msg::Path generate_path(  
           geometry_msgs::msg::PoseStamped start,  
           double spacing,  
           std::initializer_list<std::unique_ptr<PathSegment>> segments )
          {
           nav_msgs::msg::Path path;
           path.header = start.header;
           path.poses.push_back(  start );
           for (  const auto & segment : segments ) {
           segment->append(  path,   spacing );
           }
           return path;
          }
          
          } // namespace path_utils

navigation2/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp

       1  // Copyright (  c ) 2022 FireFly Automatix
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Author: Adam Aposhian
          
          #ifndef PATH_UTILS__PATH_UTILS_HPP_
          #define PATH_UTILS__PATH_UTILS_HPP_
          
          #include <cmath>
          #include <initializer_list>
          #include <memory>
          
          #include "nav_msgs/msg/path.hpp"
          
          namespace path_utils
          {
          
          /**
           * Build human-readable test paths
           */
          class PathSegment
          {
          public:
           virtual void append(  nav_msgs::msg::Path & path,   double spacing ) const = 0;
           virtual ~PathSegment(   ) {}
          };
          
      39  class Arc : public PathSegment
          {
          public:
      42   explicit Arc(  double radius,   double radians )
           : radius_(  radius ),   radians_(  radians ) {}
           void append(  nav_msgs::msg::Path & path,   double spacing ) const override;
          
          private:
           double radius_;
           double radians_;
          };
          
      51  class Straight : public PathSegment
          {
          public:
      54   explicit Straight(  double length )
           : length_(  length ) {}
           void append(  nav_msgs::msg::Path & path,   double spacing ) const override;
          
          private:
           double length_;
          };
          
      62  class LeftTurn : public Arc
          {
          public:
      65   explicit LeftTurn(  double radius )
           : Arc(  radius,   M_PI_2 ) {}
          };
          
      69  class RightTurn : public Arc
          {
          public:
           explicit RightTurn(  double radius )
           : Arc(  radius,   -M_PI_2 ) {}
          };
          
      76  class LeftTurnAround : public Arc
          {
          public:
      79   explicit LeftTurnAround(  double radius )
           : Arc(  radius,   M_PI ) {}
          };
          
      83  class RightTurnAround : public Arc
          {
          public:
           explicit RightTurnAround(  double radius )
           : Arc(  radius,   -M_PI ) {}
          };
          
      90  class LeftCircle : public Arc
          {
          public:
      93   explicit LeftCircle(  double radius )
           : Arc(  radius,   2.0 * M_PI ) {}
          };
          
      97  class RightCircle : public Arc
          {
          public:
     100   explicit RightCircle(  double radius )
           : Arc(  radius,   -2.0 * M_PI ) {}
          };
          
     104  nav_msgs::msg::Path generate_path(  
           geometry_msgs::msg::PoseStamped start,  
           double spacing,  
           std::initializer_list<std::unique_ptr<PathSegment>> segments );
          
          } // namespace path_utils
          
          #endif // PATH_UTILS__PATH_UTILS_HPP_

navigation2/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp

       1  // Copyright (  c ) 2022 Adam Aposhian
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "path_utils.hpp"
          #include "gtest/gtest.h"
          
          using namespace path_utils; // NOLINT
          
      22  TEST(  PathUtils,   test_generate_straight )
          {
           geometry_msgs::msg::PoseStamped start;
           start.header.frame_id = "test_frame";
          
           constexpr double path_length = 2.0;
           constexpr double spacing = 1.0;
          
           auto path = generate_path(  
           start,   spacing,   {
           std::make_unique<Straight>(  path_length )
           } );
           EXPECT_EQ(  path.poses.size(   ),   3u );
           for (  const auto & pose : path.poses ) {
           EXPECT_EQ(  pose.header.frame_id,   start.header.frame_id );
           }
           EXPECT_DOUBLE_EQ(  path.poses[0].pose.position.x,   0.0 );
           EXPECT_DOUBLE_EQ(  path.poses[0].pose.position.y,   0.0 );
           EXPECT_DOUBLE_EQ(  path.poses[0].pose.position.z,   0.0 );
          
           EXPECT_NEAR(  path.poses[1].pose.position.x,   1.0,   0.1 );
           EXPECT_NEAR(  path.poses[1].pose.position.y,   0.0,   0.1 );
           EXPECT_NEAR(  path.poses[1].pose.position.z,   0.0,   0.1 );
          
           EXPECT_NEAR(  path.poses[2].pose.position.x,   2.0,   0.1 );
           EXPECT_NEAR(  path.poses[2].pose.position.y,   0.0,   0.1 );
           EXPECT_NEAR(  path.poses[2].pose.position.z,   0.0,   0.1 );
          }
          
      51  TEST(  PathUtils,   test_half_turn )
          {
           // Start at a more interesting place,   turned the other way
           geometry_msgs::msg::PoseStamped start;
           start.header.frame_id = "map";
           start.pose.position.x = 1.0;
           start.pose.position.y = -1.0;
           start.pose.orientation.x = 0.0;
           start.pose.orientation.y = 0.0;
           start.pose.orientation.z = 1.0;
           start.pose.orientation.w = 0.0;
          
           constexpr double spacing = 0.1;
           constexpr double radius = 2.0;
          
           auto path = generate_path(  
           start,   spacing,   {
           std::make_unique<RightTurnAround>(  radius ),  
           } );
           constexpr double expected_path_length = M_PI * radius;
           EXPECT_NEAR(  path.poses.size(   ),   1 + static_cast<std::size_t>(  expected_path_length / spacing ),   10 );
           for (  const auto & pose : path.poses ) {
           EXPECT_EQ(  pose.header.frame_id,   start.header.frame_id );
           }
          
           // Check the last pose
           auto & last_pose = path.poses.back(   );
           auto & last_position = last_pose.pose.position;
           EXPECT_NEAR(  last_position.x,   1.0,   0.2 );
           EXPECT_NEAR(  last_position.y,   3.0,   0.2 );
           EXPECT_DOUBLE_EQ(  last_position.z,   0.0 );
          
           // Should be facing forward now
           auto & last_orientation = last_pose.pose.orientation;
           EXPECT_NEAR(  last_orientation.x,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.y,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.z,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.w,   1.0,   0.1 );
          }
          
      91  TEST(  PathUtils,   test_generate_all )
          {
           geometry_msgs::msg::PoseStamped start;
           start.header.frame_id = "map";
          
           constexpr double spacing = 0.1;
          
           auto path = generate_path(  
           start,   spacing,   {
           std::make_unique<Straight>(  1.0 ),  
           std::make_unique<LeftTurn>(  1.0 ),  
           std::make_unique<RightTurn>(  1.0 ),  
           std::make_unique<LeftTurnAround>(  1.0 ),  
           std::make_unique<RightTurnAround>(  1.0 ),  
           std::make_unique<LeftCircle>(  1.0 ),  
           std::make_unique<RightCircle>(  1.0 ),  
           std::make_unique<Arc>(  1.0,   2 * M_PI ),   // another circle
           } );
           constexpr double expected_path_length = 1.0 + 2.0 * (  M_PI_2 + M_PI_2 ) + 2.0 * (  M_PI ) +3.0 *
           (  2.0 * M_PI );
           EXPECT_NEAR(  path.poses.size(   ),   1 + static_cast<std::size_t>(  expected_path_length / spacing ),   50 );
           for (  const auto & pose : path.poses ) {
           EXPECT_EQ(  pose.header.frame_id,   start.header.frame_id );
           }
          
           // Check the last pose
           auto & last_pose = path.poses.back(   );
           auto & last_position = last_pose.pose.position;
           EXPECT_NEAR(  last_position.x,   3.0,   0.5 );
           EXPECT_NEAR(  last_position.y,   6.0,   0.5 );
           EXPECT_DOUBLE_EQ(  last_position.z,   0.0 );
          
           auto & last_orientation = last_pose.pose.orientation;
           EXPECT_NEAR(  last_orientation.x,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.y,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.z,   0.0,   0.1 );
           EXPECT_NEAR(  last_orientation.w,   1.0,   0.1 );
          }

navigation2/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "path_utils/path_utils.hpp"
          #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
          #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
          #include "nav2_core/exceptions.hpp"
          
      30  class RclCppFixture
          {
          public:
      33   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      34   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      38  class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
          {
          public:
      41   BasicAPIRPP(   )
           : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController(   ) {}
          
      44   nav_msgs::msg::Path getPlan(   ) {return global_plan_;}
          
      46   double getSpeed(   ) {return desired_linear_vel_;}
          
      48   std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsgWrapper(  
      49   const geometry_msgs::msg::PoseStamped & carrot_pose )
           {
           return createCarrotMsg(  carrot_pose );
           }
          
      54   void setVelocityScaledLookAhead(   ) {use_velocity_scaled_lookahead_dist_ = true;}
      55   void setCostRegulationScaling(   ) {use_cost_regulated_linear_velocity_scaling_ = true;}
      56   void resetVelocityRegulationScaling(   ) {use_regulated_linear_velocity_scaling_ = false;}
          
      58   double getLookAheadDistanceWrapper(  const geometry_msgs::msg::Twist & twist )
           {
           return getLookAheadDistance(  twist );
           }
          
      63   static geometry_msgs::msg::Point circleSegmentIntersectionWrapper(  
      64   const geometry_msgs::msg::Point & p1,  
      65   const geometry_msgs::msg::Point & p2,  
           double r )
           {
           return circleSegmentIntersection(  p1,   p2,   r );
           }
          
      71   geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(  
      72   const double & dist,   const nav_msgs::msg::Path & path )
           {
           return getLookAheadPoint(  dist,   path );
           }
          
      77   bool shouldRotateToPathWrapper(  
      78   const geometry_msgs::msg::PoseStamped & carrot_pose,   double & angle_to_path )
           {
           return shouldRotateToPath(  carrot_pose,   angle_to_path );
           }
          
      83   bool shouldRotateToGoalHeadingWrapper(  const geometry_msgs::msg::PoseStamped & carrot_pose )
           {
           return shouldRotateToGoalHeading(  carrot_pose );
           }
          
      88   void rotateToHeadingWrapper(  
           double & linear_vel,   double & angular_vel,  
      90   const double & angle_to_path,   const geometry_msgs::msg::Twist & curr_speed )
           {
           return rotateToHeading(  linear_vel,   angular_vel,   angle_to_path,   curr_speed );
           }
          
      95   void applyConstraintsWrapper(  
           const double & dist_error,   const double & lookahead_dist,  
      97   const double & curvature,   const geometry_msgs::msg::Twist & curr_speed,  
           const double & pose_cost,   double & linear_vel,   double & sign )
           {
           return applyConstraints(  
           dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,  
           linear_vel,   sign );
           }
          
     105   double findVelocitySignChangeWrapper(  
     106   const nav_msgs::msg::Path & transformed_plan )
           {
           return findVelocitySignChange(  transformed_plan );
           }
          
     111   nav_msgs::msg::Path transformGlobalPlanWrapper(  
     112   const geometry_msgs::msg::PoseStamped & pose )
           {
           return transformGlobalPlan(  pose );
           }
          };
          
     118  TEST(  RegulatedPurePursuitTest,   basicAPI )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPP" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
          
           // instantiate
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
           ctrl->configure(  node,   name,   tf,   costmap );
           ctrl->activate(   );
           ctrl->deactivate(   );
           ctrl->cleanup(   );
          
           // setPlan and get plan
           nav_msgs::msg::Path path;
           path.poses.resize(  2 );
           path.poses[0].header.frame_id = "fake_frame";
           ctrl->setPlan(  path );
           EXPECT_EQ(  ctrl->getPlan(   ).poses.size(   ),   2ul );
           EXPECT_EQ(  ctrl->getPlan(   ).poses[0].header.frame_id,   std::string(  "fake_frame" ) );
          
           // set speed limit
           const double base_speed = ctrl->getSpeed(   );
           EXPECT_EQ(  ctrl->getSpeed(   ),   base_speed );
           ctrl->setSpeedLimit(  0.51,   false );
           EXPECT_EQ(  ctrl->getSpeed(   ),   0.51 );
           ctrl->setSpeedLimit(  nav2_costmap_2d::NO_SPEED_LIMIT,   false );
           EXPECT_EQ(  ctrl->getSpeed(   ),   base_speed );
           ctrl->setSpeedLimit(  30,   true );
           EXPECT_EQ(  ctrl->getSpeed(   ),   base_speed * 0.3 );
           ctrl->setSpeedLimit(  nav2_costmap_2d::NO_SPEED_LIMIT,   true );
           EXPECT_EQ(  ctrl->getSpeed(   ),   base_speed );
          }
          
     154  TEST(  RegulatedPurePursuitTest,   createCarrotMsg )
          {
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           geometry_msgs::msg::PoseStamped pose;
           pose.header.frame_id = "Hi!";
           pose.pose.position.x = 1.0;
           pose.pose.position.y = 12.0;
           pose.pose.orientation.w = 0.5;
          
           auto rtn = ctrl->createCarrotMsgWrapper(  pose );
           EXPECT_EQ(  rtn->header.frame_id,   std::string(  "Hi!" ) );
           EXPECT_EQ(  rtn->point.x,   1.0 );
           EXPECT_EQ(  rtn->point.y,   12.0 );
           EXPECT_EQ(  rtn->point.z,   0.01 );
          }
          
     170  TEST(  RegulatedPurePursuitTest,   findVelocitySignChange )
          {
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPPfindVelocitySignChange" );
           geometry_msgs::msg::PoseStamped pose;
           pose.header.frame_id = "smb";
           auto time = node->get_clock(   )->now(   );
           pose.header.stamp = time;
           pose.pose.position.x = 1.0;
           pose.pose.position.y = 0.0;
          
           nav_msgs::msg::Path path;
           path.poses.resize(  3 );
           path.header.frame_id = "smb";
           path.header.stamp = pose.header.stamp;
           path.poses[0].pose.position.x = 1.0;
           path.poses[0].pose.position.y = 1.0;
           path.poses[1].pose.position.x = 2.0;
           path.poses[1].pose.position.y = 2.0;
           path.poses[2].pose.position.x = -1.0;
           path.poses[2].pose.position.y = -1.0;
           ctrl->setPlan(  path );
           auto rtn = ctrl->findVelocitySignChangeWrapper(  path );
           EXPECT_EQ(  rtn,   sqrt(  8.0 ) );
          
           path.poses[2].pose.position.x = 3.0;
           path.poses[2].pose.position.y = 3.0;
           ctrl->setPlan(  path );
           rtn = ctrl->findVelocitySignChangeWrapper(  path );
           EXPECT_EQ(  rtn,   std::numeric_limits<double>::max(   ) );
          }
          
          using CircleSegmentIntersectionParam = std::tuple<
           std::pair<double,   double>,  
           std::pair<double,   double>,  
           double,  
           std::pair<double,   double>
          >;
          
     209  class CircleSegmentIntersectionTest
     210   : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
          {};
          
     213  TEST_P(  CircleSegmentIntersectionTest,   circleSegmentIntersection )
          {
           auto pair1 = std::get<0>(  GetParam(   ) );
           auto pair2 = std::get<1>(  GetParam(   ) );
           auto r = std::get<2>(  GetParam(   ) );
           auto expected_pair = std::get<3>(  GetParam(   ) );
           auto pair_to_point = [](  std::pair<double,   double> p ) -> geometry_msgs::msg::Point {
           geometry_msgs::msg::Point point;
           point.x = p.first;
           point.y = p.second;
           point.z = 0.0;
           return point;
           };
           auto p1 = pair_to_point(  pair1 );
           auto p2 = pair_to_point(  pair2 );
           auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(  p1,   p2,   r );
           auto expected_point = pair_to_point(  expected_pair );
           EXPECT_DOUBLE_EQ(  actual.x,   expected_point.x );
           EXPECT_DOUBLE_EQ(  actual.y,   expected_point.y );
           // Expect that the intersection point is actually r away from the origin
           EXPECT_DOUBLE_EQ(  r,   std::hypot(  actual.x,   actual.y ) );
          }
          
     236  INSTANTIATE_TEST_SUITE_P(  
           InterpolationTest,  
           CircleSegmentIntersectionTest,  
           testing::Values(  
           // Origin to the positive X axis
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {2.0,   0.0},  
           1.0,  
           {1.0,   0.0}
          },  
           // Origin to hte negative X axis
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {-2.0,   0.0},  
           1.0,  
           {-1.0,   0.0}
          },  
           // Origin to the positive Y axis
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {0.0,   2.0},  
           1.0,  
           {0.0,   1.0}
          },  
           // Origin to the negative Y axis
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {0.0,   -2.0},  
           1.0,  
           {0.0,   -1.0}
          },  
           // non-origin to the X axis with non-unit circle,   with the second point inside
           CircleSegmentIntersectionParam{
           {4.0,   0.0},  
           {-1.0,   0.0},  
           2.0,  
           {2.0,   0.0}
          },  
           // non-origin to the Y axis with non-unit circle,   with the second point inside
           CircleSegmentIntersectionParam{
           {0.0,   4.0},  
           {0.0,   -0.5},  
           2.0,  
           {0.0,   2.0}
          },  
           // origin to the positive X axis,   on the circle
           CircleSegmentIntersectionParam{
           {2.0,   0.0},  
           {0.0,   0.0},  
           2.0,  
           {2.0,   0.0}
          },  
           // origin to the positive Y axis,   on the circle
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {0.0,   2.0},  
           2.0,  
           {0.0,   2.0}
          },  
           // origin to the upper-right quadrant (  3-4-5 triangle )
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {6.0,   8.0},  
           5.0,  
           {3.0,   4.0}
          },  
           // origin to the lower-left quadrant (  3-4-5 triangle )
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {-6.0,   -8.0},  
           5.0,  
           {-3.0,   -4.0}
          },  
           // origin to the upper-left quadrant (  3-4-5 triangle )
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {-6.0,   8.0},  
           5.0,  
           {-3.0,   4.0}
          },  
           // origin to the lower-right quadrant (  3-4-5 triangle )
           CircleSegmentIntersectionParam{
           {0.0,   0.0},  
           {6.0,   -8.0},  
           5.0,  
           {3.0,   -4.0}
          }
           ) );
          
     326  TEST(  RegulatedPurePursuitTest,   lookaheadAPI )
          {
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPP" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
           ctrl->configure(  node,   name,   tf,   costmap );
          
           geometry_msgs::msg::Twist twist;
          
           // test getLookAheadDistance
           double rtn = ctrl->getLookAheadDistanceWrapper(  twist );
           EXPECT_EQ(  rtn,   0.6 ); // default lookahead_dist
          
           // shouldn't be a function of speed
           twist.linear.x = 10.0;
           rtn = ctrl->getLookAheadDistanceWrapper(  twist );
           EXPECT_EQ(  rtn,   0.6 );
          
           // now it should be a function of velocity,   max out
           ctrl->setVelocityScaledLookAhead(   );
           rtn = ctrl->getLookAheadDistanceWrapper(  twist );
           EXPECT_EQ(  rtn,   0.9 ); // 10 speed maxes out at max_lookahead_dist
          
           // check normal range
           twist.linear.x = 0.35;
           rtn = ctrl->getLookAheadDistanceWrapper(  twist );
           EXPECT_NEAR(  rtn,   0.525,   0.0001 ); // 1.5 * 0.35
          
           // check minimum range
           twist.linear.x = 0.0;
           rtn = ctrl->getLookAheadDistanceWrapper(  twist );
           EXPECT_EQ(  rtn,   0.3 );
          
           // test getLookAheadPoint
           double dist = 1.0;
           nav_msgs::msg::Path path;
           path.poses.resize(  10 );
           for (  uint i = 0; i != path.poses.size(   ); i++ ) {
           path.poses[i].pose.position.x = static_cast<double>(  i );
           }
          
           // test exact hits
           auto pt = ctrl->getLookAheadPointWrapper(  dist,   path );
           EXPECT_EQ(  pt.pose.position.x,   1.0 );
          
           // test getting next closest point without interpolation
           node->set_parameter(  
           rclcpp::Parameter(  
           name + ".use_interpolation",  
           rclcpp::ParameterValue(  false ) ) );
           ctrl->configure(  node,   name,   tf,   costmap );
           dist = 3.8;
           pt = ctrl->getLookAheadPointWrapper(  dist,   path );
           EXPECT_EQ(  pt.pose.position.x,   4.0 );
          
           // test end of path
           dist = 100.0;
           pt = ctrl->getLookAheadPointWrapper(  dist,   path );
           EXPECT_EQ(  pt.pose.position.x,   9.0 );
          
           // test interpolation
           node->set_parameter(  
           rclcpp::Parameter(  
           name + ".use_interpolation",  
           rclcpp::ParameterValue(  true ) ) );
           ctrl->configure(  node,   name,   tf,   costmap );
           dist = 3.8;
           pt = ctrl->getLookAheadPointWrapper(  dist,   path );
           EXPECT_EQ(  pt.pose.position.x,   3.8 );
          }
          
     401  TEST(  RegulatedPurePursuitTest,   rotateTests )
          {
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPP" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
           ctrl->configure(  node,   name,   tf,   costmap );
          
           // shouldRotateToPath
           geometry_msgs::msg::PoseStamped carrot;
           double angle_to_path_rtn;
           EXPECT_EQ(  ctrl->shouldRotateToPathWrapper(  carrot,   angle_to_path_rtn ),   false );
          
           carrot.pose.position.x = 0.5;
           carrot.pose.position.y = 0.25;
           EXPECT_EQ(  ctrl->shouldRotateToPathWrapper(  carrot,   angle_to_path_rtn ),   false );
          
           carrot.pose.position.x = 0.5;
           carrot.pose.position.y = 1.0;
           EXPECT_EQ(  ctrl->shouldRotateToPathWrapper(  carrot,   angle_to_path_rtn ),   true );
          
           // shouldRotateToGoalHeading
           carrot.pose.position.x = 0.0;
           carrot.pose.position.y = 0.0;
           EXPECT_EQ(  ctrl->shouldRotateToGoalHeadingWrapper(  carrot ),   true );
          
           carrot.pose.position.x = 0.0;
           carrot.pose.position.y = 0.24;
           EXPECT_EQ(  ctrl->shouldRotateToGoalHeadingWrapper(  carrot ),   true );
          
           carrot.pose.position.x = 0.0;
           carrot.pose.position.y = 0.26;
           EXPECT_EQ(  ctrl->shouldRotateToGoalHeadingWrapper(  carrot ),   false );
          
           // rotateToHeading
           double lin_v = 10.0;
           double ang_v = 0.5;
           double angle_to_path = 0.4;
           geometry_msgs::msg::Twist curr_speed;
           curr_speed.angular.z = 1.75;
          
           // basic full speed at a speed
           ctrl->rotateToHeadingWrapper(  lin_v,   ang_v,   angle_to_path,   curr_speed );
           EXPECT_EQ(  lin_v,   0.0 );
           EXPECT_EQ(  ang_v,   1.8 );
          
           // negative direction
           angle_to_path = -0.4;
           curr_speed.angular.z = -1.75;
           ctrl->rotateToHeadingWrapper(  lin_v,   ang_v,   angle_to_path,   curr_speed );
           EXPECT_EQ(  ang_v,   -1.8 );
          
           // kinematic clamping,   no speed,   some speed accelerating,   some speed decelerating
           angle_to_path = 0.4;
           curr_speed.angular.z = 0.0;
           ctrl->rotateToHeadingWrapper(  lin_v,   ang_v,   angle_to_path,   curr_speed );
           EXPECT_NEAR(  ang_v,   0.16,   0.01 );
          
           curr_speed.angular.z = 1.0;
           ctrl->rotateToHeadingWrapper(  lin_v,   ang_v,   angle_to_path,   curr_speed );
           EXPECT_NEAR(  ang_v,   1.16,   0.01 );
          
           angle_to_path = -0.4;
           curr_speed.angular.z = 1.0;
           ctrl->rotateToHeadingWrapper(  lin_v,   ang_v,   angle_to_path,   curr_speed );
           EXPECT_NEAR(  ang_v,   0.84,   0.01 );
          }
          
     472  TEST(  RegulatedPurePursuitTest,   applyConstraints )
          {
           auto ctrl = std::make_shared<BasicAPIRPP>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPP" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
           ctrl->configure(  node,   name,   tf,   costmap );
          
           double dist_error = 0.0;
           double lookahead_dist = 0.6;
           double curvature = 0.5;
           geometry_msgs::msg::Twist curr_speed;
           double pose_cost = 0.0;
           double linear_vel = 0.0;
           double sign = 1.0;
          
           // test curvature regulation (  default )
           curr_speed.linear.x = 0.25;
           ctrl->applyConstraintsWrapper(  
           dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,  
           linear_vel,   sign );
           EXPECT_EQ(  linear_vel,   0.25 ); // min set speed
          
           linear_vel = 1.0;
           curvature = 0.7407;
           curr_speed.linear.x = 0.5;
           ctrl->applyConstraintsWrapper(  
           dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,  
           linear_vel,   sign );
           EXPECT_NEAR(  linear_vel,   0.5,   0.01 ); // lower by curvature
          
           linear_vel = 1.0;
           curvature = 1000.0;
           curr_speed.linear.x = 0.25;
           ctrl->applyConstraintsWrapper(  
           dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,  
           linear_vel,   sign );
           EXPECT_NEAR(  linear_vel,   0.25,   0.01 ); // min out by curvature
          
          
           // now try with cost regulation (  turn off velocity and only cost )
           // ctrl->setCostRegulationScaling(   );
           // ctrl->resetVelocityRegulationScaling(   );
           // curvature = 0.0;
          
           // min changable cost
           // pose_cost = 1;
           // linear_vel = 0.5;
           // curr_speed.linear.x = 0.5;
           // ctrl->applyConstraintsWrapper(  
           // dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,   linear_vel );
           // EXPECT_NEAR(  linear_vel,   0.498,   0.01 );
          
           // max changing cost
           // pose_cost = 127;
           // curr_speed.linear.x = 0.255;
           // ctrl->applyConstraintsWrapper(  
           // dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,   linear_vel );
           // EXPECT_NEAR(  linear_vel,   0.255,   0.01 );
          
           // over max cost thresh
           // pose_cost = 200;
           // curr_speed.linear.x = 0.25;
           // ctrl->applyConstraintsWrapper(  
           // dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,   linear_vel );
           // EXPECT_NEAR(  linear_vel,   0.25,   0.01 );
          
           // test kinematic clamping
           // pose_cost = 200;
           // curr_speed.linear.x = 1.0;
           // ctrl->applyConstraintsWrapper(  
           // dist_error,   lookahead_dist,   curvature,   curr_speed,   pose_cost,   linear_vel );
           // EXPECT_NEAR(  linear_vel,   0.5,   0.01 );
          }
          
     550  TEST(  RegulatedPurePursuitTest,   testDynamicParameter )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "Smactest" );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap->on_configure(  rclcpp_lifecycle::State(   ) );
           auto ctrl =
           std::make_unique<nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController>(   );
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           ctrl->configure(  node,   "test",   tf,   costmap );
           ctrl->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           node->get_node_base_interface(   ),   node->get_node_topics_interface(   ),  
           node->get_node_graph_interface(   ),  
           node->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.desired_linear_vel",   1.0 ),  
           rclcpp::Parameter(  "test.lookahead_dist",   7.0 ),  
           rclcpp::Parameter(  "test.max_lookahead_dist",   7.0 ),  
           rclcpp::Parameter(  "test.min_lookahead_dist",   6.0 ),  
           rclcpp::Parameter(  "test.lookahead_time",   1.8 ),  
           rclcpp::Parameter(  "test.rotate_to_heading_angular_vel",   18.0 ),  
           rclcpp::Parameter(  "test.min_approach_linear_velocity",   1.0 ),  
           rclcpp::Parameter(  "test.max_allowed_time_to_collision_up_to_carrot",   2.0 ),  
           rclcpp::Parameter(  "test.cost_scaling_dist",   2.0 ),  
           rclcpp::Parameter(  "test.cost_scaling_gain",   4.0 ),  
           rclcpp::Parameter(  "test.regulated_linear_scaling_min_radius",   10.0 ),  
           rclcpp::Parameter(  "test.transform_tolerance",   30.0 ),  
           rclcpp::Parameter(  "test.max_angular_accel",   3.0 ),  
           rclcpp::Parameter(  "test.rotate_to_heading_min_angle",   0.7 ),  
           rclcpp::Parameter(  "test.regulated_linear_scaling_min_speed",   4.0 ),  
           rclcpp::Parameter(  "test.use_velocity_scaled_lookahead_dist",   false ),  
           rclcpp::Parameter(  "test.use_regulated_linear_velocity_scaling",   false ),  
           rclcpp::Parameter(  "test.use_cost_regulated_linear_velocity_scaling",   false ),  
           rclcpp::Parameter(  "test.allow_reversing",   false ),  
           rclcpp::Parameter(  "test.use_rotate_to_heading",   false )} );
          
           rclcpp::spin_until_future_complete(  
           node->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  node->get_parameter(  "test.desired_linear_vel" ).as_double(   ),   1.0 );
           EXPECT_EQ(  node->get_parameter(  "test.lookahead_dist" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.max_lookahead_dist" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.min_lookahead_dist" ).as_double(   ),   6.0 );
           EXPECT_EQ(  node->get_parameter(  "test.lookahead_time" ).as_double(   ),   1.8 );
           EXPECT_EQ(  node->get_parameter(  "test.rotate_to_heading_angular_vel" ).as_double(   ),   18.0 );
           EXPECT_EQ(  node->get_parameter(  "test.min_approach_linear_velocity" ).as_double(   ),   1.0 );
           EXPECT_EQ(  
           node->get_parameter(  
           "test.max_allowed_time_to_collision_up_to_carrot" ).as_double(   ),   2.0 );
           EXPECT_EQ(  node->get_parameter(  "test.cost_scaling_dist" ).as_double(   ),   2.0 );
           EXPECT_EQ(  node->get_parameter(  "test.cost_scaling_gain" ).as_double(   ),   4.0 );
           EXPECT_EQ(  node->get_parameter(  "test.regulated_linear_scaling_min_radius" ).as_double(   ),   10.0 );
           EXPECT_EQ(  node->get_parameter(  "test.transform_tolerance" ).as_double(   ),   30.0 );
           EXPECT_EQ(  node->get_parameter(  "test.max_angular_accel" ).as_double(   ),   3.0 );
           EXPECT_EQ(  node->get_parameter(  "test.rotate_to_heading_min_angle" ).as_double(   ),   0.7 );
           EXPECT_EQ(  node->get_parameter(  "test.regulated_linear_scaling_min_speed" ).as_double(   ),   4.0 );
           EXPECT_EQ(  node->get_parameter(  "test.use_velocity_scaled_lookahead_dist" ).as_bool(   ),   false );
           EXPECT_EQ(  node->get_parameter(  "test.use_regulated_linear_velocity_scaling" ).as_bool(   ),   false );
           EXPECT_EQ(  
           node->get_parameter(  
           "test.use_cost_regulated_linear_velocity_scaling" ).as_bool(   ),   false );
           EXPECT_EQ(  node->get_parameter(  "test.allow_reversing" ).as_bool(   ),   false );
           EXPECT_EQ(  node->get_parameter(  "test.use_rotate_to_heading" ).as_bool(   ),   false );
          }
          
     618  class TransformGlobalPlanTest : public ::testing::Test
          {
          protected:
           void SetUp(   ) override
           {
           ctrl_ = std::make_shared<BasicAPIRPP>(   );
           node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testRPP" );
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           }
          
           void configure_costmap(  uint16_t width,   double resolution )
           {
           constexpr char costmap_frame[] = "test_costmap_frame";
     632   constexpr char robot_frame[] = "test_robot_frame";
          
     634   auto results = costmap_->set_parameters(  
           {
           rclcpp::Parameter(  "global_frame",   costmap_frame ),  
           rclcpp::Parameter(  "robot_base_frame",   robot_frame ),  
           rclcpp::Parameter(  "width",   width ),  
           rclcpp::Parameter(  "height",   width ),  
           rclcpp::Parameter(  "resolution",   resolution )
           } );
           for (  const auto & result : results ) {
           EXPECT_TRUE(  result.successful ) << result.reason;
           }
          
           rclcpp_lifecycle::State state;
           costmap_->on_configure(  state );
           }
          
           void configure_controller(  double max_robot_pose_search_dist )
           {
           std::string plugin_name = "test_rpp";
           nav2_util::declare_parameter_if_not_declared(  
           node_,   plugin_name + ".max_robot_pose_search_dist",  
           rclcpp::ParameterValue(  max_robot_pose_search_dist ) );
           ctrl_->configure(  node_,   plugin_name,   tf_buffer_,   costmap_ );
           }
          
           void setup_transforms(  geometry_msgs::msg::Point & robot_position )
           {
           transform_time_ = node_->get_clock(   )->now(   );
           // Note: transforms go parent to child
          
           // We will have a separate path and costmap frame for completeness,  
           // but we will leave them cooincident for convenience.
           geometry_msgs::msg::TransformStamped path_to_costmap;
           path_to_costmap.header.frame_id = PATH_FRAME;
           path_to_costmap.header.stamp = transform_time_;
           path_to_costmap.child_frame_id = COSTMAP_FRAME;
           path_to_costmap.transform.translation.x = 0.0;
           path_to_costmap.transform.translation.y = 0.0;
           path_to_costmap.transform.translation.z = 0.0;
          
           geometry_msgs::msg::TransformStamped costmap_to_robot;
           costmap_to_robot.header.frame_id = COSTMAP_FRAME;
           costmap_to_robot.header.stamp = transform_time_;
           costmap_to_robot.child_frame_id = ROBOT_FRAME;
           costmap_to_robot.transform.translation.x = robot_position.x;
           costmap_to_robot.transform.translation.y = robot_position.y;
           costmap_to_robot.transform.translation.z = robot_position.z;
          
           tf2_msgs::msg::TFMessage tf_message;
           tf_message.transforms = {
           path_to_costmap,  
           costmap_to_robot
           };
           for (  const auto & transform : tf_message.transforms ) {
           tf_buffer_->setTransform(  transform,   "test",   false );
           }
           tf_buffer_->setUsingDedicatedThread(  true ); // lying to let it do transforms
           }
          
           static constexpr char PATH_FRAME[] = "test_path_frame";
           static constexpr char COSTMAP_FRAME[] = "test_costmap_frame";
           static constexpr char ROBOT_FRAME[] = "test_robot_frame";
          
           std::shared_ptr<BasicAPIRPP> ctrl_;
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           rclcpp::Time transform_time_;
          };
          
          // This tests that not only should nothing get pruned on a costmap
          // that contains the entire global_plan,   and also that it doesn't skip to the end of the path
          // which is closer to the robot pose than the start.
          TEST_F(  TransformGlobalPlanTest,   no_pruning_on_large_costmap )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           robot_pose.pose.position.x = -0.1;
           robot_pose.pose.position.y = 0.0;
           robot_pose.pose.position.z = 0.0;
           // A really big costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  100u,   0.1 );
           configure_controller(  5.0 );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           constexpr double spacing = 0.1;
           constexpr double circle_radius = 1.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::LeftCircle>(  circle_radius )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
          
           auto transformed_plan = ctrl_->transformGlobalPlanWrapper(  robot_pose );
           EXPECT_EQ(  transformed_plan.poses.size(   ),   global_plan.poses.size(   ) );
          }
          
          // This plan shouldn't get pruned because of the costmap,  
          // but should be half pruned because it is halfway around the circle
          TEST_F(  TransformGlobalPlanTest,   transform_start_selection )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           robot_pose.pose.position.x = 0.0;
           robot_pose.pose.position.y = 4.0; // on the other side of the circle
           robot_pose.pose.position.z = 0.0;
           // Could set orientation going the other way,   but RPP doesn't care
           constexpr double spacing = 0.1;
           constexpr double circle_radius = 2.0; // diameter 4
          
           // A really big costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  100u,   0.1 );
           // This should just be at least half the circumference: pi*r ~= 6
           constexpr double max_robot_pose_search_dist = 10.0;
           configure_controller(  max_robot_pose_search_dist );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::LeftCircle>(  circle_radius )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
           auto transformed_plan = ctrl_->transformGlobalPlanWrapper(  robot_pose );
           EXPECT_NEAR(  transformed_plan.poses.size(   ),   global_plan.poses.size(   ) / 2,   1 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.x,   0.0,   0.5 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.y,   0.0,   0.5 );
          }
          
          // This should throw an exception when all poses are outside of the costmap
          TEST_F(  TransformGlobalPlanTest,   all_poses_outside_of_costmap )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           // far away from the path
           robot_pose.pose.position.x = 1000.0;
           robot_pose.pose.position.y = 1000.0;
           robot_pose.pose.position.z = 0.0;
           // Could set orientation going the other way,   but RPP doesn't care
           constexpr double spacing = 0.1;
           constexpr double circle_radius = 2.0; // diameter 4
          
           // A "normal" costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  10u,   0.1 );
           // This should just be at least half the circumference: pi*r ~= 6
           constexpr double max_robot_pose_search_dist = 10.0;
           configure_controller(  max_robot_pose_search_dist );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::LeftCircle>(  circle_radius )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
           EXPECT_THROW(  ctrl_->transformGlobalPlanWrapper(  robot_pose ),   nav2_core::PlannerException );
          }
          
          // Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist
          TEST_F(  TransformGlobalPlanTest,   good_circle_shortcut )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           // far away from the path
           robot_pose.pose.position.x = -0.1;
           robot_pose.pose.position.y = 0.0;
           robot_pose.pose.position.z = 0.0;
           // Could set orientation going the other way,   but RPP doesn't care
           constexpr double spacing = 0.1;
           constexpr double circle_radius = 2.0; // diameter 4
          
           // A "normal" costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  100u,   0.1 );
           // This should just be at least the circumference: 2*pi*r ~= 12
           constexpr double max_robot_pose_search_dist = 15.0;
           configure_controller(  max_robot_pose_search_dist );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::LeftCircle>(  circle_radius )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
           auto transformed_plan = ctrl_->transformGlobalPlanWrapper(  robot_pose );
           EXPECT_NEAR(  transformed_plan.poses.size(   ),   1,   1 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.x,   0.0,   0.5 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.y,   0.0,   0.5 );
          }
          
          // Simple costmap pruning on a straight line
          TEST_F(  TransformGlobalPlanTest,   costmap_pruning )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           // far away from the path
           robot_pose.pose.position.x = -0.1;
           robot_pose.pose.position.y = 0.0;
           robot_pose.pose.position.z = 0.0;
           // Could set orientation going the other way,   but RPP doesn't care
           constexpr double spacing = 1.0;
          
           // A "normal" costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  20u,   0.5 );
           constexpr double max_robot_pose_search_dist = 10.0;
           configure_controller(  max_robot_pose_search_dist );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           constexpr double path_length = 100.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::Straight>(  path_length )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
           auto transformed_plan = ctrl_->transformGlobalPlanWrapper(  robot_pose );
           EXPECT_NEAR(  transformed_plan.poses.size(   ),   10u,   1 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.x,   0.0,   0.5 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.y,   0.0,   0.5 );
          }
          
          // Should prune out later portions of the path that come back into the costmap
          TEST_F(  TransformGlobalPlanTest,   prune_after_leaving_costmap )
          {
           geometry_msgs::msg::PoseStamped robot_pose;
           robot_pose.header.frame_id = COSTMAP_FRAME;
           robot_pose.header.stamp = transform_time_;
           // far away from the path
           robot_pose.pose.position.x = -0.1;
           robot_pose.pose.position.y = 0.0;
           robot_pose.pose.position.z = 0.0;
           // Could set orientation going the other way,   but RPP doesn't care
           constexpr double spacing = 1.0;
          
           // A "normal" costmap
           // the max_costmap_extent should be 50m
           configure_costmap(  20u,   0.5 );
           constexpr double max_robot_pose_search_dist = 10.0;
           configure_controller(  max_robot_pose_search_dist );
           setup_transforms(  robot_pose.pose.position );
          
           // Set up test path;
          
           geometry_msgs::msg::PoseStamped start_of_path;
           start_of_path.header.frame_id = PATH_FRAME;
           start_of_path.header.stamp = transform_time_;
           start_of_path.pose.position.x = 0.0;
           start_of_path.pose.position.y = 0.0;
           start_of_path.pose.position.z = 0.0;
          
           constexpr double path_length = 100.0;
          
           auto global_plan = path_utils::generate_path(  
           start_of_path,   spacing,   {
           std::make_unique<path_utils::Straight>(  path_length ),  
           std::make_unique<path_utils::LeftTurnAround>(  1.0 ),  
           std::make_unique<path_utils::Straight>(  path_length )
           } );
          
           ctrl_->setPlan(  global_plan );
          
           // Transform the plan
           auto transformed_plan = ctrl_->transformGlobalPlanWrapper(  robot_pose );
           // This should be essentially the same as the regular straight path
           EXPECT_NEAR(  transformed_plan.poses.size(   ),   10u,   1 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.x,   0.0,   0.5 );
           EXPECT_NEAR(  transformed_plan.poses[0].pose.position.y,   0.0,   0.5 );
          }

navigation2/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
          #define NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
          
          #include <string>
          #include <vector>
          #include <memory>
          #include <algorithm>
          #include <mutex>
          
          #include "rclcpp/rclcpp.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_core/controller.hpp"
          #include "nav2_core/exceptions.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "angles/angles.h"
          
          namespace nav2_rotation_shim_controller
          {
          
          /**
           * @class nav2_rotation_shim_controller::RotationShimController
           * @brief Rotate to rough path heading controller shim plugin
           */
      42  class RotationShimController : public nav2_core::Controller
          {
          public:
           /**
           * @brief Constructor for nav2_rotation_shim_controller::RotationShimController
           */
      48   RotationShimController(   );
          
           /**
           * @brief Destrructor for nav2_rotation_shim_controller::RotationShimController
           */
           ~RotationShimController(   ) override = default;
          
           /**
           * @brief Configure controller state machine
           * @param parent WeakPtr to node
           * @param name Name of plugin
           * @param tf TF buffer
           * @param costmap_ros Costmap2DROS object of environment
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup controller state machine
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate controller state machine
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate controller state machine
           */
           void deactivate(   ) override;
          
           /**
           * @brief Compute the best command given the current pose and velocity
           * @param pose Current robot pose
           * @param velocity Current robot velocity
           * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
           * @return Best command
           */
           geometry_msgs::msg::TwistStamped computeVelocityCommands(  
           const geometry_msgs::msg::PoseStamped & pose,  
           const geometry_msgs::msg::Twist & velocity,  
           nav2_core::GoalChecker * /*goal_checker*/ ) override;
          
           /**
           * @brief nav2_core setPlan - Sets the global plan
           * @param path The global plan
           */
           void setPlan(  const nav_msgs::msg::Path & path ) override;
          
           /**
           * @brief Limits the maximum linear speed of the robot.
           * @param speed_limit expressed in absolute value (  in m/s )
           * or in percentage from maximum robot speed.
           * @param percentage Setting speed limit in percentage if true
           * or in absolute values in false case.
           */
           void setSpeedLimit(  const double & speed_limit,   const bool & percentage ) override;
          
          protected:
           /**
           * @brief Finds the point on the path that is roughly the sampling
           * point distance away from the robot for use.
           * May throw exception if a point at least that far away cannot be found
           * @return pt location of the output point
           */
           geometry_msgs::msg::PoseStamped getSampledPathPt(   );
          
           /**
           * @brief Uses TF to find the location of the sampled path point in base frame
           * @param pt location of the sampled path point
           * @return location of the pose in base frame
           */
           geometry_msgs::msg::Pose transformPoseToBaseFrame(  const geometry_msgs::msg::PoseStamped & pt );
          
           /**
           * @brief Rotates the robot to the rough heading
           * @param angular_distance Angular distance to the goal remaining
           * @param pose Starting pose of robot
           * @param velocity Starting velocity of robot
           * @return Twist command for rotation to rough heading
           */
           geometry_msgs::msg::TwistStamped computeRotateToHeadingCommand(  
           const double & angular_distance,  
           const geometry_msgs::msg::PoseStamped & pose,  
           const geometry_msgs::msg::Twist & velocity );
          
           /**
           * @brief Checks if rotation is safe
           * @param cmd_vel Velocity to check over
           * @param angular_distance_to_heading Angular distance to heading requested
           * @param pose Starting pose of robot
           */
           void isCollisionFree(  
           const geometry_msgs::msg::TwistStamped & cmd_vel,  
           const double & angular_distance_to_heading,  
           const geometry_msgs::msg::PoseStamped & pose );
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::string plugin_name_;
           rclcpp::Logger logger_ {rclcpp::get_logger(  "RotationShimController" )};
           rclcpp::Clock::SharedPtr clock_;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
     161   std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
           collision_checker_;
          
           pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
           nav2_core::Controller::Ptr primary_controller_;
           bool path_updated_;
           nav_msgs::msg::Path current_path_;
           double forward_sampling_distance_,   angular_dist_threshold_;
           double rotate_to_heading_angular_vel_,   max_angular_accel_;
           double control_duration_,   simulate_ahead_time_;
          
           // Dynamic parameters handler
           std::mutex mutex_;
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_rotation_shim_controller
          
          #endif // NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_

navigation2/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <algorithm>
          #include <string>
          #include <limits>
          #include <memory>
          #include <vector>
          #include <utility>
          
          #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
          
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_rotation_shim_controller
          {
          
      29  RotationShimController::RotationShimController(   )
          : lp_loader_(  "nav2_core",   "nav2_core::Controller" ),  
           primary_controller_(  nullptr ),  
           path_updated_(  false )
          {
          }
          
      36  void RotationShimController::configure(  
      37   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      38   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      39   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           plugin_name_ = name;
           node_ = parent;
           auto node = parent.lock(   );
          
           tf_ = tf;
           costmap_ros_ = costmap_ros;
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
          
           std::string primary_controller;
           double control_frequency;
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".angular_dist_threshold",   rclcpp::ParameterValue(  0.785 ) ); // 45 deg
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".forward_sampling_distance",   rclcpp::ParameterValue(  0.5 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".rotate_to_heading_angular_vel",   rclcpp::ParameterValue(  1.8 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".max_angular_accel",   rclcpp::ParameterValue(  3.2 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".simulate_ahead_time",   rclcpp::ParameterValue(  1.0 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name_ + ".primary_controller",   rclcpp::PARAMETER_STRING );
          
           node->get_parameter(  plugin_name_ + ".angular_dist_threshold",   angular_dist_threshold_ );
           node->get_parameter(  plugin_name_ + ".forward_sampling_distance",   forward_sampling_distance_ );
           node->get_parameter(  
           plugin_name_ + ".rotate_to_heading_angular_vel",  
           rotate_to_heading_angular_vel_ );
           node->get_parameter(  plugin_name_ + ".max_angular_accel",   max_angular_accel_ );
           node->get_parameter(  plugin_name_ + ".simulate_ahead_time",   simulate_ahead_time_ );
          
           primary_controller = node->get_parameter(  plugin_name_ + ".primary_controller" ).as_string(   );
           node->get_parameter(  "controller_frequency",   control_frequency );
           control_duration_ = 1.0 / control_frequency;
          
           try {
           primary_controller_ = lp_loader_.createUniqueInstance(  primary_controller );
           RCLCPP_INFO(  
           logger_,   "Created internal controller for rotation shimming: %s of type %s",  
           plugin_name_.c_str(   ),   primary_controller.c_str(   ) );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           logger_,  
           "Failed to create internal controller for rotation shimming. Exception: %s",   ex.what(   ) );
           return;
           }
          
           primary_controller_->configure(  parent,   name,   tf,   costmap_ros );
          
           // initialize collision checker and set costmap
           collision_checker_ = std::make_unique<nav2_costmap_2d::
           FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(  costmap_ros->getCostmap(   ) );
          }
          
      96  void RotationShimController::activate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Activating controller: %s of type "
           "nav2_rotation_shim_controller::RotationShimController",  
           plugin_name_.c_str(   ) );
          
           primary_controller_->activate(   );
          
           auto node = node_.lock(   );
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  
           &RotationShimController::dynamicParametersCallback,  
           this,   std::placeholders::_1 ) );
          }
          
     113  void RotationShimController::deactivate(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Deactivating controller: %s of type "
           "nav2_rotation_shim_controller::RotationShimController",  
           plugin_name_.c_str(   ) );
          
           primary_controller_->deactivate(   );
          
           dyn_params_handler_.reset(   );
          }
          
     126  void RotationShimController::cleanup(   )
          {
           RCLCPP_INFO(  
           logger_,  
           "Cleaning up controller: %s of type "
           "nav2_rotation_shim_controller::RotationShimController",  
           plugin_name_.c_str(   ) );
          
           primary_controller_->cleanup(   );
           primary_controller_.reset(   );
          }
          
     138  geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands(  
     139   const geometry_msgs::msg::PoseStamped & pose,  
     140   const geometry_msgs::msg::Twist & velocity,  
     141   nav2_core::GoalChecker * goal_checker )
          {
           if (  path_updated_ ) {
           std::lock_guard<std::mutex> lock_reinit(  mutex_ );
           try {
           geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(  getSampledPathPt(   ) );
          
           double angular_distance_to_heading =
           std::atan2(  sampled_pt_base.position.y,   sampled_pt_base.position.x );
           if (  fabs(  angular_distance_to_heading ) > angular_dist_threshold_ ) {
           RCLCPP_DEBUG(  
           logger_,  
           "Robot is not within the new path's rough heading,   rotating to heading..." );
           return computeRotateToHeadingCommand(  angular_distance_to_heading,   pose,   velocity );
           } else {
           RCLCPP_DEBUG(  
           logger_,  
           "Robot is at the new path's rough heading,   passing to controller" );
           path_updated_ = false;
           }
           } catch (  const std::runtime_error & e ) {
           RCLCPP_DEBUG(  
           logger_,  
           "Rotation Shim Controller was unable to find a sampling point,  "
           " a rotational collision was detected,   or TF failed to transform"
           " into base frame! what(   ): %s",   e.what(   ) );
           path_updated_ = false;
           }
           }
          
           // If at this point,   use the primary controller to path track
           return primary_controller_->computeVelocityCommands(  pose,   velocity,   goal_checker );
          }
          
     175  geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt(   )
          {
           if (  current_path_.poses.size(   ) < 2 ) {
           throw nav2_core::PlannerException(  
           "Path is too short to find a valid sampled path point for rotation." );
           }
          
           geometry_msgs::msg::Pose start = current_path_.poses.front(   ).pose;
           double dx,   dy;
          
           // Find the first point at least sampling distance away
           for (  unsigned int i = 1; i != current_path_.poses.size(   ); i++ ) {
           dx = current_path_.poses[i].pose.position.x - start.position.x;
           dy = current_path_.poses[i].pose.position.y - start.position.y;
           if (  hypot(  dx,   dy ) >= forward_sampling_distance_ ) {
           current_path_.poses[i].header.frame_id = current_path_.header.frame_id;
           current_path_.poses[i].header.stamp = clock_->now(   ); // Get current time transformation
           return current_path_.poses[i];
           }
           }
          
           throw nav2_core::PlannerException(  
           std::string(  
           "Unable to find a sampling point at least %0.2f from the robot,  "
           "passing off to primary controller plugin.",   forward_sampling_distance_ ) );
          }
          
          geometry_msgs::msg::Pose
     203  RotationShimController::transformPoseToBaseFrame(  const geometry_msgs::msg::PoseStamped & pt )
          {
           geometry_msgs::msg::PoseStamped pt_base;
           if (  !nav2_util::transformPoseInTargetFrame(  pt,   pt_base,   *tf_,   costmap_ros_->getBaseFrameID(   ) ) ) {
           throw nav2_core::PlannerException(  "Failed to transform pose to base frame!" );
           }
           return pt_base.pose;
          }
          
          geometry_msgs::msg::TwistStamped
     213  RotationShimController::computeRotateToHeadingCommand(  
           const double & angular_distance_to_heading,  
     215   const geometry_msgs::msg::PoseStamped & pose,  
     216   const geometry_msgs::msg::Twist & velocity )
          {
           geometry_msgs::msg::TwistStamped cmd_vel;
           cmd_vel.header = pose.header;
           const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
           const double angular_vel = sign * rotate_to_heading_angular_vel_;
           const double & dt = control_duration_;
           const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt;
           const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt;
           cmd_vel.twist.angular.z =
           std::clamp(  angular_vel,   min_feasible_angular_speed,   max_feasible_angular_speed );
          
           isCollisionFree(  cmd_vel,   angular_distance_to_heading,   pose );
           return cmd_vel;
          }
          
     232  void RotationShimController::isCollisionFree(  
     233   const geometry_msgs::msg::TwistStamped & cmd_vel,  
           const double & angular_distance_to_heading,  
     235   const geometry_msgs::msg::PoseStamped & pose )
          {
           // Simulate rotation ahead by time in control frequency increments
           double simulated_time = 0.0;
           double initial_yaw = tf2::getYaw(  pose.pose.orientation );
           double yaw = 0.0;
           double footprint_cost = 0.0;
           double remaining_rotation_before_thresh =
           fabs(  angular_distance_to_heading ) - angular_dist_threshold_;
          
           while (  simulated_time < simulate_ahead_time_ ) {
           simulated_time += control_duration_;
           yaw = initial_yaw + cmd_vel.twist.angular.z * simulated_time;
          
           // Stop simulating past the point it would be passed onto the primary controller
           if (  angles::shortest_angular_distance(  yaw,   initial_yaw ) >= remaining_rotation_before_thresh ) {
           break;
           }
          
           using namespace nav2_costmap_2d; // NOLINT
           footprint_cost = collision_checker_->footprintCostAtPose(  
           pose.pose.position.x,   pose.pose.position.y,  
           yaw,   costmap_ros_->getRobotFootprint(   ) );
          
           if (  footprint_cost == static_cast<double>(  NO_INFORMATION ) &&
           costmap_ros_->getLayeredCostmap(   )->isTrackingUnknown(   ) )
           {
           throw std::runtime_error(  "RotationShimController detected a potential collision ahead!" );
           }
          
           if (  footprint_cost >= static_cast<double>(  LETHAL_OBSTACLE ) ) {
           throw std::runtime_error(  "RotationShimController detected collision ahead!" );
           }
           }
          }
          
     271  void RotationShimController::setPlan(  const nav_msgs::msg::Path & path )
          {
           path_updated_ = true;
           current_path_ = path;
           primary_controller_->setPlan(  path );
          }
          
     278  void RotationShimController::setSpeedLimit(  const double & speed_limit,   const bool & percentage )
          {
           primary_controller_->setSpeedLimit(  speed_limit,   percentage );
          }
          
          rcl_interfaces::msg::SetParametersResult
     284  RotationShimController::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           std::lock_guard<std::mutex> lock_reinit(  mutex_ );
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == plugin_name_ + ".angular_dist_threshold" ) {
           angular_dist_threshold_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".forward_sampling_distance" ) {
           forward_sampling_distance_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".rotate_to_heading_angular_vel" ) {
           rotate_to_heading_angular_vel_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".max_angular_accel" ) {
           max_angular_accel_ = parameter.as_double(   );
           } else if (  name == plugin_name_ + ".simulate_ahead_time" ) {
           simulate_ahead_time_ = parameter.as_double(   );
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_rotation_shim_controller
          
          // Register this controller as a nav2_core plugin
     315  PLUGINLIB_EXPORT_CLASS(  
           nav2_rotation_shim_controller::RotationShimController,  
           nav2_core::Controller )

navigation2/nav2_rotation_shim_controller/test/test_shim_controller.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_controller/plugins/simple_goal_checker.hpp"
          #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
          #include "tf2_ros/transform_broadcaster.h"
          
      29  class RclCppFixture
          {
          public:
      32   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      33   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      37  class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController
          {
          public:
      40   RotationShimShim(   )
           : nav2_rotation_shim_controller::RotationShimController(   )
           {
           }
          
      45   nav2_core::Controller::Ptr getPrimaryController(   )
           {
           return primary_controller_;
           }
          
      50   nav_msgs::msg::Path getPath(   )
           {
           return current_path_;
           }
          
      55   bool isPathUpdated(   )
           {
           return path_updated_;
           }
          
      60   geometry_msgs::msg::PoseStamped getSampledPathPtWrapper(   )
           {
           return getSampledPathPt(   );
           }
          
      65   geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(  geometry_msgs::msg::PoseStamped pt )
           {
           return transformPoseToBaseFrame(  pt );
           }
          
           geometry_msgs::msg::TwistStamped
      71   computeRotateToHeadingCommandWrapper(  
           const double & param,  
      73   const geometry_msgs::msg::PoseStamped & pose,  
      74   const geometry_msgs::msg::Twist & velocity )
           {
           return computeRotateToHeadingCommand(  param,   pose,   velocity );
           }
          };
          
      80  TEST(  RotationShimControllerTest,   lifecycleTransitions )
          {
           auto ctrl = std::make_shared<RotationShimShim>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ShimControllerTest" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
          
           // Should not populate primary controller,   does not exist
           EXPECT_THROW(  ctrl->configure(  node,   name,   tf,   costmap ),   std::runtime_error );
           EXPECT_EQ(  ctrl->getPrimaryController(   ),   nullptr );
          
           // Add a controller to the setup
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           node->get_node_base_interface(   ),   node->get_node_topics_interface(   ),  
           node->get_node_graph_interface(   ),  
           node->get_node_services_interface(   ) );
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  
           "PathFollower.primary_controller",  
           std::string(  "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) )} );
           rclcpp::spin_until_future_complete(  
           node->get_node_base_interface(   ),  
           results );
          
           ctrl->configure(  node,   name,   tf,   costmap );
           EXPECT_NE(  ctrl->getPrimaryController(   ),   nullptr );
          
           ctrl->activate(   );
          
           ctrl->setSpeedLimit(  50.0,   2.0 );
          
           ctrl->deactivate(   );
           ctrl->cleanup(   );
          }
          
     118  TEST(  RotationShimControllerTest,   setPlanAndSampledPointsTests )
          {
           auto ctrl = std::make_shared<RotationShimShim>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ShimControllerTest" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
          
           // set a valid primary controller so we can do lifecycle
           node->declare_parameter(  
           "PathFollower.primary_controller",  
           std::string(  "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
          
           auto controller = std::make_shared<RotationShimShim>(   );
           controller->configure(  node,   name,   tf,   costmap );
           controller->activate(   );
          
           // Test state update and path setting
           nav_msgs::msg::Path path;
           path.header.frame_id = "hi mate!";
           path.poses.resize(  10 );
           path.poses[1].pose.position.x = 0.1;
           path.poses[1].pose.position.y = 0.1;
           path.poses[2].pose.position.x = 1.0;
           path.poses[2].pose.position.y = 1.0;
           path.poses[3].pose.position.x = 10.0;
           path.poses[3].pose.position.y = 10.0;
           EXPECT_EQ(  controller->isPathUpdated(   ),   false );
           controller->setPlan(  path );
           EXPECT_EQ(  controller->getPath(   ).header.frame_id,   std::string(  "hi mate!" ) );
           EXPECT_EQ(  controller->getPath(   ).poses.size(   ),   10u );
           EXPECT_EQ(  controller->isPathUpdated(   ),   true );
          
           // Test getting a sampled point
           auto pose = controller->getSampledPathPtWrapper(   );
           EXPECT_EQ(  pose.pose.position.x,   1.0 ); // default forward sampling is 0.5
           EXPECT_EQ(  pose.pose.position.y,   1.0 );
          
           nav_msgs::msg::Path path_invalid_leng;
           controller->setPlan(  path_invalid_leng );
           EXPECT_THROW(  controller->getSampledPathPtWrapper(   ),   std::runtime_error );
          
           nav_msgs::msg::Path path_invalid_dists;
           path.poses.resize(  10 );
           controller->setPlan(  path_invalid_dists );
           EXPECT_THROW(  controller->getSampledPathPtWrapper(   ),   std::runtime_error );
          }
          
     168  TEST(  RotationShimControllerTest,   rotationAndTransformTests )
          {
           auto ctrl = std::make_shared<RotationShimShim>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ShimControllerTest" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
          
           // set a valid primary controller so we can do lifecycle
           node->declare_parameter(  
           "PathFollower.primary_controller",  
           std::string(  "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
          
           auto controller = std::make_shared<RotationShimShim>(   );
           controller->configure(  node,   name,   tf,   costmap );
           controller->activate(   );
          
           // Test state update and path setting
           nav_msgs::msg::Path path;
           path.header.frame_id = "fake_frame";
           path.poses.resize(  10 );
           path.poses[1].pose.position.x = 0.1;
           path.poses[1].pose.position.y = 0.1;
           path.poses[2].pose.position.x = 1.0;
           path.poses[2].pose.position.y = 1.0;
           path.poses[3].pose.position.x = 10.0;
           path.poses[3].pose.position.y = 10.0;
           controller->setPlan(  path );
          
           const geometry_msgs::msg::Twist velocity;
           EXPECT_EQ(  
           controller->computeRotateToHeadingCommandWrapper(  
           0.7,   path.poses[0],   velocity ).twist.angular.z,   1.8 );
           EXPECT_EQ(  
           controller->computeRotateToHeadingCommandWrapper(  
           -0.7,   path.poses[0],   velocity ).twist.angular.z,   -1.8 );
          
           EXPECT_EQ(  
           controller->computeRotateToHeadingCommandWrapper(  
           0.87,   path.poses[0],   velocity ).twist.angular.z,   1.8 );
          
           // in base_link,   so should pass through values without issue
           geometry_msgs::msg::PoseStamped pt;
           pt.pose.position.x = 100.0;
           pt.header.frame_id = "base_link";
           pt.header.stamp = rclcpp::Time(   );
           auto rtn = controller->transformPoseToBaseFrameWrapper(  pt );
           EXPECT_EQ(  rtn.position.x,   100.0 );
          
           // in frame that doesn't exist,   shouldn't throw,   but should fail
           geometry_msgs::msg::PoseStamped pt2;
           pt.pose.position.x = 100.0;
           pt.header.frame_id = "fake_frame2";
           pt.header.stamp = rclcpp::Time(   );
           EXPECT_THROW(  controller->transformPoseToBaseFrameWrapper(  pt2 ),   std::runtime_error );
          }
          
     227  TEST(  RotationShimControllerTest,   computeVelocityTests )
          {
           auto ctrl = std::make_shared<RotationShimShim>(   );
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ShimControllerTest" );
           std::string name = "PathFollower";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           auto listener = std::make_shared<tf2_ros::TransformListener>(  *tf,   node,   true );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "fake_costmap" );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
           auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(  node );
          
           geometry_msgs::msg::TransformStamped transform;
           transform.header.frame_id = "base_link";
           transform.child_frame_id = "odom";
           transform.transform.rotation.x = 0.0;
           transform.transform.rotation.y = 0.0;
           transform.transform.rotation.z = 0.0;
           transform.transform.rotation.w = 1.0;
           tf_broadcaster->sendTransform(  transform );
          
           // set a valid primary controller so we can do lifecycle
           node->declare_parameter(  
           "PathFollower.primary_controller",  
           std::string(  "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
          
           auto controller = std::make_shared<RotationShimShim>(   );
           controller->configure(  node,   name,   tf,   costmap );
           controller->activate(   );
          
           // Test state update and path setting
           nav_msgs::msg::Path path;
           path.header.frame_id = "fake_frame";
           path.poses.resize(  10 );
          
           geometry_msgs::msg::PoseStamped pose;
           pose.header.frame_id = "base_link";
           geometry_msgs::msg::Twist velocity;
           nav2_controller::SimpleGoalChecker checker;
           checker.initialize(  node,   "checker",   costmap );
          
           // send without setting a path - should go to RPP immediately
           // then it should throw an exception because the path is empty and invalid
           EXPECT_THROW(  controller->computeVelocityCommands(  pose,   velocity,   &checker ),   std::runtime_error );
          
           // Set with a path -- should attempt to find a sampled point but throw exception
           // because it cannot be found,   then go to RPP and throw exception because it cannot be transformed
           controller->setPlan(  path );
           EXPECT_THROW(  controller->computeVelocityCommands(  pose,   velocity,   &checker ),   std::runtime_error );
          
           path.header.frame_id = "base_link";
           path.poses[1].pose.position.x = 0.1;
           path.poses[1].pose.position.y = 0.1;
           path.poses[2].pose.position.x = -1.0;
           path.poses[2].pose.position.y = -1.0;
           path.poses[2].header.frame_id = "base_link";
           path.poses[3].pose.position.x = 10.0;
           path.poses[3].pose.position.y = 10.0;
          
           // this should allow it to find the sampled point,   then transform to base_link
           // validly because we setup the TF for it. The -1.0 should be selected since default min
           // is 0.5 and that should cause a rotation in place
           controller->setPlan(  path );
           tf_broadcaster->sendTransform(  transform );
           auto effort = controller->computeVelocityCommands(  pose,   velocity,   &checker );
           EXPECT_EQ(  fabs(  effort.twist.angular.z ),   1.8 );
          
           path.header.frame_id = "base_link";
           path.poses[1].pose.position.x = 0.1;
           path.poses[1].pose.position.y = 0.1;
           path.poses[2].pose.position.x = 1.0;
           path.poses[2].pose.position.y = 0.0;
           path.poses[2].header.frame_id = "base_link";
           path.poses[3].pose.position.x = 10.0;
           path.poses[3].pose.position.y = 10.0;
          
           // this should allow it to find the sampled point,   then transform to base_link
           // validly because we setup the TF for it. The 1.0 should be selected since default min
           // is 0.5 and that should cause a pass off to the RPP controller which will throw
           // and exception because the costmap is bogus
           controller->setPlan(  path );
           tf_broadcaster->sendTransform(  transform );
           EXPECT_THROW(  controller->computeVelocityCommands(  pose,   velocity,   &checker ),   std::runtime_error );
          }
          
     312  TEST(  RotationShimControllerTest,   testDynamicParameter )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ShimControllerTest" );
           auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           std::string name = "test";
           auto tf = std::make_shared<tf2_ros::Buffer>(  node->get_clock(   ) );
           rclcpp_lifecycle::State state;
           costmap->on_configure(  state );
          
           // set a valid primary controller so we can do lifecycle
           node->declare_parameter(  
           "test.primary_controller",  
           std::string(  "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
          
           auto controller = std::make_shared<RotationShimShim>(   );
           controller->configure(  node,   name,   tf,   costmap );
           controller->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           node->get_node_base_interface(   ),   node->get_node_topics_interface(   ),  
           node->get_node_graph_interface(   ),  
           node->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.angular_dist_threshold",   7.0 ),  
           rclcpp::Parameter(  "test.forward_sampling_distance",   7.0 ),  
           rclcpp::Parameter(  "test.rotate_to_heading_angular_vel",   7.0 ),  
           rclcpp::Parameter(  "test.max_angular_accel",   7.0 ),  
           rclcpp::Parameter(  "test.simulate_ahead_time",   7.0 ),  
           rclcpp::Parameter(  "test.primary_controller",   std::string(  "HI" ) )} );
          
           rclcpp::spin_until_future_complete(  
           node->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  node->get_parameter(  "test.angular_dist_threshold" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.forward_sampling_distance" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.rotate_to_heading_angular_vel" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.max_angular_accel" ).as_double(   ),   7.0 );
           EXPECT_EQ(  node->get_parameter(  "test.simulate_ahead_time" ).as_double(   ),   7.0 );
          }

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/goal_common.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
          #define NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
          
          #include "nav2_rviz_plugins/goal_pose_updater.hpp"
          
          namespace nav2_rviz_plugins
          {
          extern GoalPoseUpdater GoalUpdater;
          } // nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/goal_pose_updater.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
          #define NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
          
          #include <QObject>
          
          namespace nav2_rviz_plugins
          {
          
          /// Class to set and update goal pose by emitting signal
      24  class GoalPoseUpdater : public QObject
          {
           Q_OBJECT
          
          public:
           GoalPoseUpdater(   ) {}
           ~GoalPoseUpdater(   ) {}
          
           void setGoal(  double x,   double y,   double theta,   QString frame )
           {
           emit updateGoal(  x,   y,   theta,   frame );
           }
          
      37  signals:
           void updateGoal(  double x,   double y,   double theta,   QString frame );
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/goal_tool.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
          #define NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
          
          #include <QObject>
          
          #include <memory>
          
          #include "rviz_default_plugins/tools/pose/pose_tool.hpp"
          #include "rviz_default_plugins/visibility_control.hpp"
          
          namespace rviz_common
          {
          
      28  class DisplayContext;
          
          namespace properties
          {
      32  class StringProperty;
          } // namespace properties
          } // namespace rviz_common
          
      36  namespace nav2_rviz_plugins
          {
          
          class RVIZ_DEFAULT_PLUGINS_PUBLIC GoalTool : public rviz_default_plugins::tools::PoseTool
          {
           Q_OBJECT
          
          public:
           GoalTool(   );
           ~GoalTool(   ) override;
          
           void onInitialize(   ) override;
          
          protected:
           void onPoseSet(  double x,   double y,   double theta ) override;
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
          #define NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
          
          #include <QtWidgets>
          #include <QBasicTimer>
          #undef NO_ERROR
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "nav2_msgs/action/navigate_through_poses.hpp"
          #include "nav2_msgs/action/follow_waypoints.hpp"
          #include "nav2_rviz_plugins/ros_action_qevent.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "rviz_common/panel.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "visualization_msgs/msg/marker_array.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
      38  class QPushButton;
          
          namespace nav2_rviz_plugins
          {
          
      43  class InitialThread;
          
          /// Panel to interface to the nav2 stack
      46  class Nav2Panel : public rviz_common::Panel
          {
           Q_OBJECT
          
          public:
           explicit Nav2Panel(  QWidget * parent = 0 );
           virtual ~Nav2Panel(   );
          
           void onInitialize(   ) override;
          
           /// Load and save configuration data
           void load(  const rviz_common::Config & config ) override;
           void save(  rviz_common::Config config ) const override;
          
          private Q_SLOTS:
           void startThread(   );
           void onStartup(   );
           void onShutdown(   );
           void onCancel(   );
           void onPause(   );
           void onResume(   );
           void onAccumulatedWp(   );
           void onAccumulatedNTP(   );
           void onAccumulating(   );
           void onNewGoal(  double x,   double y,   double theta,   QString frame );
          
          private:
           void loadLogFiles(   );
           void onCancelButtonPressed(   );
           void timerEvent(  QTimerEvent * event ) override;
          
           int unique_id {0};
          
           // Call to send NavigateToPose action request for goal poses
      80   void startWaypointFollowing(  std::vector<geometry_msgs::msg::PoseStamped> poses );
      81   void startNavigation(  geometry_msgs::msg::PoseStamped );
      82   void startNavThroughPoses(  std::vector<geometry_msgs::msg::PoseStamped> poses );
           using NavigationGoalHandle =
           rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
           using WaypointFollowerGoalHandle =
           rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>;
           using NavThroughPosesGoalHandle =
           rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateThroughPoses>;
          
           // The (  non-spinning ) client node used to invoke the action client
           rclcpp::Node::SharedPtr client_node_;
          
           // Timeout value when waiting for action servers to respnd
           std::chrono::milliseconds server_timeout_;
          
           // A timer used to check on the completion status of the action
           QBasicTimer timer_;
          
           // The NavigateToPose action client
           rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr navigation_action_client_;
           rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SharedPtr
           waypoint_follower_action_client_;
           rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SharedPtr
           nav_through_poses_action_client_;
          
           // Navigation action feedback subscribers
           rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>::SharedPtr
           navigation_feedback_sub_;
           rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>::SharedPtr
           nav_through_poses_feedback_sub_;
           rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::GoalStatusMessage>::SharedPtr
           navigation_goal_status_sub_;
           rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::GoalStatusMessage>::SharedPtr
           nav_through_poses_goal_status_sub_;
          
           // Goal-related state
           nav2_msgs::action::NavigateToPose::Goal navigation_goal_;
           nav2_msgs::action::FollowWaypoints::Goal waypoint_follower_goal_;
           nav2_msgs::action::NavigateThroughPoses::Goal nav_through_poses_goal_;
           NavigationGoalHandle::SharedPtr navigation_goal_handle_;
           WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
           NavThroughPosesGoalHandle::SharedPtr nav_through_poses_goal_handle_;
          
           // The client used to control the nav2 stack
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
          
     128   QPushButton * start_reset_button_{nullptr};
     129   QPushButton * pause_resume_button_{nullptr};
           QPushButton * navigation_mode_button_{nullptr};
          
           QLabel * navigation_status_indicator_{nullptr};
           QLabel * localization_status_indicator_{nullptr};
           QLabel * navigation_goal_status_indicator_{nullptr};
           QLabel * navigation_feedback_indicator_{nullptr};
          
           QStateMachine state_machine_;
           InitialThread * initial_thread_;
          
           QState * pre_initial_{nullptr};
           QState * initial_{nullptr};
           QState * idle_{nullptr};
           QState * reset_{nullptr};
           QState * paused_{nullptr};
           QState * resumed_{nullptr};
           // The following states are added to allow for the state of the button to only expose reset
           // while the NavigateToPoses action is not active. While running,   the user will be allowed to
           // cancel the action. The ROSActionTransition allows for the state of the action to be detected
           // and the button state to change automatically.
           QState * running_{nullptr};
           QState * canceled_{nullptr};
           // The following states are added to allow to collect several poses to perform a waypoint-mode
           // navigation or navigate through poses mode.
           QState * accumulating_{nullptr};
           QState * accumulated_wp_{nullptr};
           QState * accumulated_nav_through_poses_{nullptr};
          
           std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
          
           // Publish the visual markers with the waypoints
           void updateWpNavigationMarkers(   );
          
           // Create unique id numbers for markers
           int getUniqueId(   );
          
           void resetUniqueId(   );
          
           // create label string from goal status msg
           static inline QString getGoalStatusLabel(  
           int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN );
          
           // create label string from feedback msg
           static inline QString getNavToPoseFeedbackLabel(  
           nav2_msgs::action::NavigateToPose::Feedback msg =
           nav2_msgs::action::NavigateToPose::Feedback(   ) );
           static inline QString getNavThroughPosesFeedbackLabel(  
           nav2_msgs::action::NavigateThroughPoses::Feedback =
           nav2_msgs::action::NavigateThroughPoses::Feedback(   ) );
           template<typename T>
           static inline std::string toLabel(  T & msg );
          
           // round off double to the specified precision and convert to string
           static inline std::string toString(  double val,   int precision = 0 );
          
           // Waypoint navigation visual markers publisher
           rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr wp_navigation_markers_pub_;
          };
          
          class InitialThread : public QThread
          {
           Q_OBJECT
          
          public:
           using SystemStatus = nav2_lifecycle_manager::SystemStatus;
          
           explicit InitialThread(  
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_nav,  
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_loc )
           : client_nav_(  client_nav ),   client_loc_(  client_loc )
           {}
          
           void run(   ) override
           {
           SystemStatus status_nav = SystemStatus::TIMEOUT;
           SystemStatus status_loc = SystemStatus::TIMEOUT;
          
           while (  status_nav == SystemStatus::TIMEOUT ) {
           if (  status_nav == SystemStatus::TIMEOUT ) {
           status_nav = client_nav_->is_active(  std::chrono::seconds(  1 ) );
           }
           }
          
           // try to communicate twice,   might not actually be up if in SLAM mode
           bool tried_loc_bringup_once = false;
           while (  status_loc == SystemStatus::TIMEOUT ) {
           status_loc = client_loc_->is_active(  std::chrono::seconds(  1 ) );
           if (  tried_loc_bringup_once ) {
           break;
           }
           tried_loc_bringup_once = true;
           }
          
           if (  status_nav == SystemStatus::ACTIVE ) {
           emit navigationActive(   );
           } else {
           emit navigationInactive(   );
           }
          
           if (  status_loc == SystemStatus::ACTIVE ) {
           emit localizationActive(   );
           } else {
           emit localizationInactive(   );
           }
           }
          
          signals:
           void navigationActive(   );
           void navigationInactive(   );
           void localizationActive(   );
           void localizationInactive(   );
          
          private:
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
           std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * Copyright (  c ) 2018,   Bosch Software Innovations GmbH.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
          #define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
          
          #include <vector>
          
          #include <OgreManualObject.h>
          #include <OgreMaterialManager.h>
          #include <OgreSceneNode.h>
          #include <OgreVector3.h>
          #include <OgreQuaternion.h>
          
          #include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
          
          namespace nav2_rviz_plugins
          {
          
          struct OgrePoseWithWeight;
          
      64  class FlatWeightedArrowsArray
          {
          public:
      67   explicit FlatWeightedArrowsArray(  Ogre::SceneManager * scene_manager_ );
      68   ~FlatWeightedArrowsArray(   );
          
      70   void createAndAttachManualObject(  Ogre::SceneNode * scene_node );
      71   void updateManualObject(  
      72   Ogre::ColourValue color,  
           float alpha,  
           float min_length,  
           float max_length,  
      76   const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses );
      77   void clear(   );
          
          private:
      80   void setManualObjectMaterial(   );
      81   void setManualObjectVertices(  
      82   const Ogre::ColourValue & color,  
           float min_length,  
           float max_length,  
      85   const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses );
          
      87   Ogre::SceneManager * scene_manager_;
      88   Ogre::ManualObject * manual_object_;
      89   Ogre::MaterialPtr material_;
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp

          /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * Copyright (  c ) 2018,   Bosch Software Innovations GmbH.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
          #define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
          
          #include <memory>
          #include <vector>
          
          #include "nav2_msgs/msg/particle_cloud.hpp"
          
          #include "rviz_rendering/objects/shape.hpp"
          #include "rviz_common/message_filter_display.hpp"
          
          namespace Ogre
          {
      59  class ManualObject;
          } // namespace Ogre
          
          namespace rviz_common
          {
          namespace properties
          {
      66  class EnumProperty;
      67  class ColorProperty;
      68  class FloatProperty;
          } // namespace properties
          } // namespace rviz_common
          
          namespace rviz_rendering
          {
      74  class Arrow;
      75  class Axes;
          } // namespace rviz_rendering
          
          namespace nav2_rviz_plugins
          {
      80  class FlatWeightedArrowsArray;
          struct OgrePoseWithWeight
          {
           Ogre::Vector3 position;
           Ogre::Quaternion orientation;
           float weight;
          };
          
          /** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */
      89  class ParticleCloudDisplay : public rviz_common::MessageFilterDisplay<nav2_msgs::msg::ParticleCloud>
          {
           Q_OBJECT
          
          public:
           // TODO(  botteroa-si ): Constructor for testing,   remove once ros_nodes can be mocked and call
           // initialize instead
           ParticleCloudDisplay(  
           rviz_common::DisplayContext * display_context,  
           Ogre::SceneNode * scene_node );
      99   ParticleCloudDisplay(   );
     100   ~ParticleCloudDisplay(   ) override;
          
     102   void processMessage(  nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg ) override;
     103   void setShape(  QString shape ); // for testing
          
          protected:
     106   void onInitialize(   ) override;
     107   void reset(   ) override;
          
     109  private Q_SLOTS:
           /// Update the interface and visible shapes based on the selected shape type.
           void updateShapeChoice(   );
          
           /// Update the arrow color.
     114   void updateArrowColor(   );
          
           /// Update arrow geometry
     117   void updateGeometry(   );
          
          private:
           void initializeProperties(   );
           bool validateFloats(  const nav2_msgs::msg::ParticleCloud & msg );
           bool setTransform(  std_msgs::msg::Header const & header );
           void updateDisplay(   );
           void updateArrows2d(   );
           void updateArrows3d(   );
           void updateAxes(   );
           void updateArrow3dGeometry(   );
           void updateAxesGeometry(   );
          
           std::unique_ptr<rviz_rendering::Axes> makeAxes(   );
           std::unique_ptr<rviz_rendering::Arrow> makeArrow3d(   );
          
           std::vector<OgrePoseWithWeight> poses_;
           std::unique_ptr<FlatWeightedArrowsArray> arrows2d_;
           std::vector<std::unique_ptr<rviz_rendering::Arrow>> arrows3d_;
           std::vector<std::unique_ptr<rviz_rendering::Axes>> axes_;
          
           Ogre::SceneNode * arrow_node_;
           Ogre::SceneNode * axes_node_;
          
           rviz_common::properties::EnumProperty * shape_property_;
           rviz_common::properties::ColorProperty * arrow_color_property_;
           rviz_common::properties::FloatProperty * arrow_alpha_property_;
          
           rviz_common::properties::FloatProperty * arrow_min_length_property_;
           rviz_common::properties::FloatProperty * arrow_max_length_property_;
          
           float min_length_;
           float max_length_;
           float length_scale_;
           float head_radius_scale_;
           float head_length_scale_;
           float shaft_radius_scale_;
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/ros_action_qevent.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
          #define NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
          
          #include <QAbstractTransition>
          
          namespace nav2_rviz_plugins
          {
          
      23  enum class QActionState
          {
           ACTIVE,  
           INACTIVE
          };
          
          /// Custom Event to track state of ROS Action
          struct ROSActionQEvent : public QEvent
          {
           explicit ROSActionQEvent(  QActionState state )
           : QEvent(  QEvent::Type(  QEvent::User + 1 ) ),  
           state_(  state ) {}
          
           QActionState state_;
          };
          
          /// Custom Transition to check whether ROS Action state has changed
      40  class ROSActionQTransition : public QAbstractTransition
          {
          public:
      43   explicit ROSActionQTransition(  QActionState initial_status )
           : status_(  initial_status )
           {}
          
      47   ~ROSActionQTransition(   ) {}
          
          protected:
      50   virtual bool eventTest(  QEvent * e )
           {
           if (  e->type(   ) != QEvent::Type(  QEvent::User + 1 ) ) { // ROSActionEvent
           return false;
           }
           ROSActionQEvent * action_event = static_cast<ROSActionQEvent *>(  e );
           return status_ != action_event->state_;
           }
          
      59   virtual void onTransition(  QEvent * ) {}
      60   QActionState status_;
          };
          
          } // namespace nav2_rviz_plugins
          
          #endif // NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_

navigation2/nav2_rviz_plugins/src/goal_tool.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_rviz_plugins/goal_tool.hpp"
          
          #include <memory>
          #include <string>
          
          #include "nav2_rviz_plugins/goal_common.hpp"
          #include "rviz_common/display_context.hpp"
          #include "rviz_common/load_resource.hpp"
          
          namespace nav2_rviz_plugins
          {
          
      27  GoalTool::GoalTool(   )
          : rviz_default_plugins::tools::PoseTool(   )
          {
           shortcut_key_ = 'g';
          }
          
      33  GoalTool::~GoalTool(   )
          {
          }
          
      37  void GoalTool::onInitialize(   )
          {
           PoseTool::onInitialize(   );
           setName(  "Nav2 Goal" );
           setIcon(  rviz_common::loadPixmap(  "package://rviz_default_plugins/icons/classes/SetGoal.png" ) );
          }
          
          void
      45  GoalTool::onPoseSet(  double x,   double y,   double theta )
          {
           // Set goal pose on global object GoalUpdater to update nav2 Panel
           GoalUpdater.setGoal(  x,   y,   theta,   context_->getFixedFrame(   ) );
          }
          
          } // namespace nav2_rviz_plugins
          
          #include <pluginlib/class_list_macros.hpp> // NOLINT
      54  PLUGINLIB_EXPORT_CLASS(  nav2_rviz_plugins::GoalTool,   rviz_common::Tool )

navigation2/nav2_rviz_plugins/src/nav2_panel.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_rviz_plugins/nav2_panel.hpp"
          
          #include <QtConcurrent/QtConcurrent>
          #include <QVBoxLayout>
          
          #include <memory>
          #include <vector>
          #include <utility>
          #include <chrono>
          #include <string>
          
          #include "nav2_rviz_plugins/goal_common.hpp"
          #include "rviz_common/display_context.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_rviz_plugins
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
          // Define global GoalPoseUpdater so that the nav2 GoalTool plugin can access to update goal pose
          GoalPoseUpdater GoalUpdater;
          
      39  Nav2Panel::Nav2Panel(  QWidget * parent )
          : Panel(  parent ),  
           server_timeout_(  100 )
          {
           // Create the control button and its tooltip
          
           start_reset_button_ = new QPushButton;
           pause_resume_button_ = new QPushButton;
           navigation_mode_button_ = new QPushButton;
           navigation_status_indicator_ = new QLabel;
           localization_status_indicator_ = new QLabel;
           navigation_goal_status_indicator_ = new QLabel;
           navigation_feedback_indicator_ = new QLabel;
          
           // Create the state machine used to present the proper control button states in the UI
          
           const char * startup_msg = "Configure and activate all nav2 lifecycle nodes";
           const char * shutdown_msg = "Deactivate and cleanup all nav2 lifecycle nodes";
           const char * cancel_msg = "Cancel navigation";
           const char * pause_msg = "Deactivate all nav2 lifecycle nodes";
           const char * resume_msg = "Activate all nav2 lifecycle nodes";
           const char * single_goal_msg = "Change to waypoint / nav through poses style navigation";
           const char * waypoint_goal_msg = "Start following waypoints";
           const char * nft_goal_msg = "Start navigating through poses";
           const char * cancel_waypoint_msg = "Cancel waypoint / viapoint accumulation mode";
          
           const QString navigation_active(  "<table><tr><td width=100><b>Navigation:</b></td>"
           "<td><font color=green>active</color></td></tr></table>" );
           const QString navigation_inactive(  "<table><tr><td width=100><b>Navigation:</b></td>"
           "<td>inactive</td></tr></table>" );
           const QString navigation_unknown(  "<table><tr><td width=100><b>Navigation:</b></td>"
           "<td>unknown</td></tr></table>" );
           const QString localization_active(  "<table><tr><td width=100><b>Localization:</b></td>"
           "<td><font color=green>active</color></td></tr></table>" );
           const QString localization_inactive(  "<table><tr><td width=100><b>Localization:</b></td>"
           "<td>inactive</td></tr></table>" );
           const QString localization_unknown(  "<table><tr><td width=100><b>Localization:</b></td>"
           "<td>unknown</td></tr></table>" );
          
           navigation_status_indicator_->setText(  navigation_unknown );
           localization_status_indicator_->setText(  localization_unknown );
           navigation_goal_status_indicator_->setText(  getGoalStatusLabel(   ) );
           navigation_feedback_indicator_->setText(  getNavThroughPosesFeedbackLabel(   ) );
           navigation_status_indicator_->setSizePolicy(  QSizePolicy::Fixed,   QSizePolicy::Fixed );
           localization_status_indicator_->setSizePolicy(  QSizePolicy::Fixed,   QSizePolicy::Fixed );
           navigation_goal_status_indicator_->setSizePolicy(  QSizePolicy::Fixed,   QSizePolicy::Fixed );
           navigation_feedback_indicator_->setSizePolicy(  QSizePolicy::Fixed,   QSizePolicy::Fixed );
          
           pre_initial_ = new QState(   );
           pre_initial_->setObjectName(  "pre_initial" );
           pre_initial_->assignProperty(  start_reset_button_,   "text",   "Startup" );
           pre_initial_->assignProperty(  start_reset_button_,   "enabled",   false );
          
           pre_initial_->assignProperty(  pause_resume_button_,   "text",   "Pause" );
           pre_initial_->assignProperty(  pause_resume_button_,   "enabled",   false );
          
           pre_initial_->assignProperty(  
           navigation_mode_button_,   "text",  
           "Waypoint / Nav Through Poses Mode" );
           pre_initial_->assignProperty(  navigation_mode_button_,   "enabled",   false );
          
           initial_ = new QState(   );
           initial_->setObjectName(  "initial" );
           initial_->assignProperty(  start_reset_button_,   "text",   "Startup" );
           initial_->assignProperty(  start_reset_button_,   "toolTip",   startup_msg );
           initial_->assignProperty(  start_reset_button_,   "enabled",   true );
          
           initial_->assignProperty(  pause_resume_button_,   "text",   "Pause" );
           initial_->assignProperty(  pause_resume_button_,   "enabled",   false );
          
           initial_->assignProperty(  navigation_mode_button_,   "text",   "Waypoint / Nav Through Poses Mode" );
           initial_->assignProperty(  navigation_mode_button_,   "enabled",   false );
          
           // State entered when navigate_to_pose action is not active
           idle_ = new QState(   );
           idle_->setObjectName(  "idle" );
           idle_->assignProperty(  start_reset_button_,   "text",   "Reset" );
           idle_->assignProperty(  start_reset_button_,   "toolTip",   shutdown_msg );
           idle_->assignProperty(  start_reset_button_,   "enabled",   true );
          
           idle_->assignProperty(  pause_resume_button_,   "text",   "Pause" );
           idle_->assignProperty(  pause_resume_button_,   "enabled",   true );
           idle_->assignProperty(  pause_resume_button_,   "toolTip",   pause_msg );
          
           idle_->assignProperty(  navigation_mode_button_,   "text",   "Waypoint / Nav Through Poses Mode" );
           idle_->assignProperty(  navigation_mode_button_,   "enabled",   true );
           idle_->assignProperty(  navigation_mode_button_,   "toolTip",   single_goal_msg );
          
           // State entered when navigate_to_pose action is not active
           accumulating_ = new QState(   );
           accumulating_->setObjectName(  "accumulating" );
           accumulating_->assignProperty(  start_reset_button_,   "text",   "Cancel Accumulation" );
           accumulating_->assignProperty(  start_reset_button_,   "toolTip",   cancel_waypoint_msg );
           accumulating_->assignProperty(  start_reset_button_,   "enabled",   true );
          
           accumulating_->assignProperty(  pause_resume_button_,   "text",   "Start Nav Through Poses" );
           accumulating_->assignProperty(  pause_resume_button_,   "enabled",   true );
           accumulating_->assignProperty(  pause_resume_button_,   "toolTip",   nft_goal_msg );
          
           accumulating_->assignProperty(  navigation_mode_button_,   "text",   "Start Waypoint Following" );
           accumulating_->assignProperty(  navigation_mode_button_,   "enabled",   true );
           accumulating_->assignProperty(  navigation_mode_button_,   "toolTip",   waypoint_goal_msg );
          
           accumulated_wp_ = new QState(   );
           accumulated_wp_->setObjectName(  "accumulated_wp" );
           accumulated_wp_->assignProperty(  start_reset_button_,   "text",   "Cancel" );
           accumulated_wp_->assignProperty(  start_reset_button_,   "toolTip",   cancel_msg );
           accumulated_wp_->assignProperty(  start_reset_button_,   "enabled",   true );
          
           accumulated_wp_->assignProperty(  pause_resume_button_,   "text",   "Start Nav Through Poses" );
           accumulated_wp_->assignProperty(  pause_resume_button_,   "enabled",   false );
           accumulated_wp_->assignProperty(  pause_resume_button_,   "toolTip",   nft_goal_msg );
          
           accumulated_wp_->assignProperty(  navigation_mode_button_,   "text",   "Start Waypoint Following" );
           accumulated_wp_->assignProperty(  navigation_mode_button_,   "enabled",   false );
           accumulated_wp_->assignProperty(  navigation_mode_button_,   "toolTip",   waypoint_goal_msg );
          
           accumulated_nav_through_poses_ = new QState(   );
           accumulated_nav_through_poses_->setObjectName(  "accumulated_nav_through_poses" );
           accumulated_nav_through_poses_->assignProperty(  start_reset_button_,   "text",   "Cancel" );
           accumulated_nav_through_poses_->assignProperty(  start_reset_button_,   "toolTip",   cancel_msg );
           accumulated_nav_through_poses_->assignProperty(  start_reset_button_,   "enabled",   true );
          
           accumulated_nav_through_poses_->assignProperty(  
           pause_resume_button_,   "text",  
           "Start Nav Through Poses" );
           accumulated_nav_through_poses_->assignProperty(  pause_resume_button_,   "enabled",   false );
           accumulated_nav_through_poses_->assignProperty(  pause_resume_button_,   "toolTip",   nft_goal_msg );
          
           accumulated_nav_through_poses_->assignProperty(  
           navigation_mode_button_,   "text",  
           "Start Waypoint Following" );
           accumulated_nav_through_poses_->assignProperty(  navigation_mode_button_,   "enabled",   false );
           accumulated_nav_through_poses_->assignProperty(  
           navigation_mode_button_,   "toolTip",  
           waypoint_goal_msg );
          
           // State entered to cancel the navigate_to_pose action
           canceled_ = new QState(   );
           canceled_->setObjectName(  "canceled" );
          
           // State entered to reset the nav2 lifecycle nodes
           reset_ = new QState(   );
           reset_->setObjectName(  "reset" );
          
           // State entered while the navigate_to_pose action is active
           running_ = new QState(   );
           running_->setObjectName(  "running" );
           running_->assignProperty(  start_reset_button_,   "text",   "Cancel" );
           running_->assignProperty(  start_reset_button_,   "toolTip",   cancel_msg );
          
           running_->assignProperty(  pause_resume_button_,   "text",   "Pause" );
           running_->assignProperty(  pause_resume_button_,   "enabled",   false );
          
           running_->assignProperty(  navigation_mode_button_,   "text",   "Waypoint mode" );
           running_->assignProperty(  navigation_mode_button_,   "enabled",   false );
          
           // State entered when pause is requested
           paused_ = new QState(   );
           paused_->setObjectName(  "pausing" );
           paused_->assignProperty(  start_reset_button_,   "text",   "Reset" );
           paused_->assignProperty(  start_reset_button_,   "toolTip",   shutdown_msg );
          
           paused_->assignProperty(  pause_resume_button_,   "text",   "Resume" );
           paused_->assignProperty(  pause_resume_button_,   "toolTip",   resume_msg );
           paused_->assignProperty(  pause_resume_button_,   "enabled",   true );
          
           paused_->assignProperty(  navigation_mode_button_,   "text",   "Start navigation" );
           paused_->assignProperty(  navigation_mode_button_,   "toolTip",   resume_msg );
           paused_->assignProperty(  navigation_mode_button_,   "enabled",   true );
          
           // State entered to resume the nav2 lifecycle nodes
           resumed_ = new QState(   );
           resumed_->setObjectName(  "resuming" );
          
           QObject::connect(  initial_,   SIGNAL(  exited(   ) ),   this,   SLOT(  onStartup(   ) ) );
           QObject::connect(  canceled_,   SIGNAL(  exited(   ) ),   this,   SLOT(  onCancel(   ) ) );
           QObject::connect(  reset_,   SIGNAL(  exited(   ) ),   this,   SLOT(  onShutdown(   ) ) );
           QObject::connect(  paused_,   SIGNAL(  entered(   ) ),   this,   SLOT(  onPause(   ) ) );
           QObject::connect(  resumed_,   SIGNAL(  exited(   ) ),   this,   SLOT(  onResume(   ) ) );
           QObject::connect(  accumulating_,   SIGNAL(  entered(   ) ),   this,   SLOT(  onAccumulating(   ) ) );
           QObject::connect(  accumulated_wp_,   SIGNAL(  entered(   ) ),   this,   SLOT(  onAccumulatedWp(   ) ) );
           QObject::connect(  
           accumulated_nav_through_poses_,   SIGNAL(  entered(   ) ),   this,  
           SLOT(  onAccumulatedNTP(   ) ) );
          
           // Start/Reset button click transitions
           initial_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   idle_ );
           idle_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   reset_ );
           running_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   canceled_ );
           paused_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   reset_ );
           idle_->addTransition(  navigation_mode_button_,   SIGNAL(  clicked(   ) ),   accumulating_ );
           accumulating_->addTransition(  navigation_mode_button_,   SIGNAL(  clicked(   ) ),   accumulated_wp_ );
           accumulating_->addTransition(  
           pause_resume_button_,   SIGNAL(  
           clicked(   ) ),   accumulated_nav_through_poses_ );
           accumulating_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   idle_ );
           accumulated_wp_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   canceled_ );
           accumulated_nav_through_poses_->addTransition(  start_reset_button_,   SIGNAL(  clicked(   ) ),   canceled_ );
          
           // Internal state transitions
           canceled_->addTransition(  canceled_,   SIGNAL(  entered(   ) ),   idle_ );
           reset_->addTransition(  reset_,   SIGNAL(  entered(   ) ),   initial_ );
           resumed_->addTransition(  resumed_,   SIGNAL(  entered(   ) ),   idle_ );
          
           // Pause/Resume button click transitions
           idle_->addTransition(  pause_resume_button_,   SIGNAL(  clicked(   ) ),   paused_ );
           paused_->addTransition(  pause_resume_button_,   SIGNAL(  clicked(   ) ),   resumed_ );
          
           // ROSAction Transitions: So when actions are updated remotely (  failing,   succeeding,   etc )
           // the state of the application will also update. This means that if in the processing
           // states and then goes inactive,   move back to the idle state. Vise versa as well.
           ROSActionQTransition * idleTransition = new ROSActionQTransition(  QActionState::INACTIVE );
           idleTransition->setTargetState(  running_ );
           idle_->addTransition(  idleTransition );
          
           ROSActionQTransition * runningTransition = new ROSActionQTransition(  QActionState::ACTIVE );
           runningTransition->setTargetState(  idle_ );
           running_->addTransition(  runningTransition );
          
           ROSActionQTransition * idleAccumulatedWpTransition =
           new ROSActionQTransition(  QActionState::INACTIVE );
           idleAccumulatedWpTransition->setTargetState(  accumulated_wp_ );
           idle_->addTransition(  idleAccumulatedWpTransition );
          
           ROSActionQTransition * accumulatedWpTransition = new ROSActionQTransition(  QActionState::ACTIVE );
           accumulatedWpTransition->setTargetState(  idle_ );
           accumulated_wp_->addTransition(  accumulatedWpTransition );
          
           ROSActionQTransition * idleAccumulatedNTPTransition =
           new ROSActionQTransition(  QActionState::INACTIVE );
           idleAccumulatedNTPTransition->setTargetState(  accumulated_nav_through_poses_ );
           idle_->addTransition(  idleAccumulatedNTPTransition );
          
           ROSActionQTransition * accumulatedNTPTransition = new ROSActionQTransition(  QActionState::ACTIVE );
           accumulatedNTPTransition->setTargetState(  idle_ );
           accumulated_nav_through_poses_->addTransition(  accumulatedNTPTransition );
          
           auto options = rclcpp::NodeOptions(   ).arguments(  
           {"--ros-args --remap __node:=navigation_dialog_action_client"} );
           client_node_ = std::make_shared<rclcpp::Node>(  "_",   options );
          
           client_nav_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(  
           "lifecycle_manager_navigation",   client_node_ );
           client_loc_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(  
           "lifecycle_manager_localization",   client_node_ );
           initial_thread_ = new InitialThread(  client_nav_,   client_loc_ );
           connect(  initial_thread_,   &InitialThread::finished,   initial_thread_,   &QObject::deleteLater );
          
           QSignalTransition * activeSignal = new QSignalTransition(  
           initial_thread_,  
           &InitialThread::navigationActive );
           activeSignal->setTargetState(  idle_ );
           pre_initial_->addTransition(  activeSignal );
          
           QSignalTransition * inactiveSignal = new QSignalTransition(  
           initial_thread_,  
           &InitialThread::navigationInactive );
           inactiveSignal->setTargetState(  initial_ );
           pre_initial_->addTransition(  inactiveSignal );
          
           QObject::connect(  
           initial_thread_,   &InitialThread::navigationActive,  
           [this,   navigation_active] {
           navigation_status_indicator_->setText(  navigation_active );
           } );
           QObject::connect(  
           initial_thread_,   &InitialThread::navigationInactive,  
           [this,   navigation_inactive] {
           navigation_status_indicator_->setText(  navigation_inactive );
           navigation_goal_status_indicator_->setText(  getGoalStatusLabel(   ) );
           navigation_feedback_indicator_->setText(  getNavThroughPosesFeedbackLabel(   ) );
           } );
           QObject::connect(  
           initial_thread_,   &InitialThread::localizationActive,  
           [this,   localization_active] {
           localization_status_indicator_->setText(  localization_active );
           } );
           QObject::connect(  
           initial_thread_,   &InitialThread::localizationInactive,  
           [this,   localization_inactive] {
           localization_status_indicator_->setText(  localization_inactive );
           } );
          
           state_machine_.addState(  pre_initial_ );
           state_machine_.addState(  initial_ );
           state_machine_.addState(  idle_ );
           state_machine_.addState(  running_ );
           state_machine_.addState(  canceled_ );
           state_machine_.addState(  reset_ );
           state_machine_.addState(  paused_ );
           state_machine_.addState(  resumed_ );
           state_machine_.addState(  accumulating_ );
           state_machine_.addState(  accumulated_wp_ );
           state_machine_.addState(  accumulated_nav_through_poses_ );
          
           state_machine_.setInitialState(  pre_initial_ );
          
           // delay starting initial thread until state machine has started or a race occurs
           QObject::connect(  &state_machine_,   SIGNAL(  started(   ) ),   this,   SLOT(  startThread(   ) ) );
           state_machine_.start(   );
          
           // Lay out the items in the panel
           QVBoxLayout * main_layout = new QVBoxLayout;
           main_layout->addWidget(  navigation_status_indicator_ );
           main_layout->addWidget(  localization_status_indicator_ );
           main_layout->addWidget(  navigation_goal_status_indicator_ );
           main_layout->addWidget(  navigation_feedback_indicator_ );
           main_layout->addWidget(  pause_resume_button_ );
           main_layout->addWidget(  start_reset_button_ );
           main_layout->addWidget(  navigation_mode_button_ );
          
           main_layout->setContentsMargins(  10,   10,   10,   10 );
           setLayout(  main_layout );
          
           navigation_action_client_ =
           rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(  
           client_node_,  
           "navigate_to_pose" );
           waypoint_follower_action_client_ =
           rclcpp_action::create_client<nav2_msgs::action::FollowWaypoints>(  
           client_node_,  
           "follow_waypoints" );
           nav_through_poses_action_client_ =
           rclcpp_action::create_client<nav2_msgs::action::NavigateThroughPoses>(  
           client_node_,  
           "navigate_through_poses" );
           navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal(   );
           waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal(   );
           nav_through_poses_goal_ = nav2_msgs::action::NavigateThroughPoses::Goal(   );
          
           wp_navigation_markers_pub_ =
           client_node_->create_publisher<visualization_msgs::msg::MarkerArray>(  
           "waypoints",  
           rclcpp::QoS(  1 ).transient_local(   ) );
          
           QObject::connect(  
           &GoalUpdater,   SIGNAL(  updateGoal(  double,  double,  double,  QString ) ),   // NOLINT
           this,   SLOT(  onNewGoal(  double,  double,  double,  QString ) ) ); // NOLINT
          }
          
     380  Nav2Panel::~Nav2Panel(   )
          {
          }
          
          void
     385  Nav2Panel::onInitialize(   )
          {
           auto node = getDisplayContext(   )->getRosNodeAbstraction(   ).lock(   )->get_raw_node(   );
          
           // create action feedback subscribers
           navigation_feedback_sub_ =
           node->create_subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>(  
           "navigate_to_pose/_action/feedback",  
           rclcpp::SystemDefaultsQoS(   ),  
           [this](  const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::SharedPtr msg ) {
           navigation_feedback_indicator_->setText(  getNavToPoseFeedbackLabel(  msg->feedback ) );
           } );
           nav_through_poses_feedback_sub_ =
           node->create_subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>(  
           "navigate_through_poses/_action/feedback",  
           rclcpp::SystemDefaultsQoS(   ),  
           [this](  const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg ) {
           navigation_feedback_indicator_->setText(  getNavThroughPosesFeedbackLabel(  msg->feedback ) );
           } );
          
           // create action goal status subscribers
           navigation_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(  
           "navigate_to_pose/_action/status",  
           rclcpp::SystemDefaultsQoS(   ),  
           [this](  const action_msgs::msg::GoalStatusArray::SharedPtr msg ) {
           navigation_goal_status_indicator_->setText(  
           getGoalStatusLabel(  msg->status_list.back(   ).status ) );
           if (  msg->status_list.back(   ).status != action_msgs::msg::GoalStatus::STATUS_EXECUTING ) {
           navigation_feedback_indicator_->setText(  getNavToPoseFeedbackLabel(   ) );
           }
           } );
           nav_through_poses_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(  
           "navigate_through_poses/_action/status",  
           rclcpp::SystemDefaultsQoS(   ),  
           [this](  const action_msgs::msg::GoalStatusArray::SharedPtr msg ) {
           navigation_goal_status_indicator_->setText(  
           getGoalStatusLabel(  msg->status_list.back(   ).status ) );
           if (  msg->status_list.back(   ).status != action_msgs::msg::GoalStatus::STATUS_EXECUTING ) {
           navigation_feedback_indicator_->setText(  getNavThroughPosesFeedbackLabel(   ) );
           }
           } );
          }
          
          void
     429  Nav2Panel::startThread(   )
          {
           // start initial thread now that state machine is started
           initial_thread_->start(   );
          }
          
          void
     436  Nav2Panel::onPause(   )
          {
           QFuture<void> futureNav =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::pause,  
           client_nav_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
           QFuture<void> futureLoc =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::pause,  
           client_loc_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
          }
          
          void
     451  Nav2Panel::onResume(   )
          {
           QFuture<void> futureNav =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::resume,  
           client_nav_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
           QFuture<void> futureLoc =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::resume,  
           client_loc_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
          }
          
          void
     466  Nav2Panel::onStartup(   )
          {
           QFuture<void> futureNav =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::startup,  
           client_nav_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
           QFuture<void> futureLoc =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::startup,  
           client_loc_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
          }
          
          void
     481  Nav2Panel::onShutdown(   )
          {
           QFuture<void> futureNav =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::reset,  
           client_nav_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
           QFuture<void> futureLoc =
           QtConcurrent::run(  
           std::bind(  
           &nav2_lifecycle_manager::LifecycleManagerClient::reset,  
           client_loc_.get(   ),   std::placeholders::_1 ),   server_timeout_ );
           timer_.stop(   );
          }
          
          void
     497  Nav2Panel::onCancel(   )
          {
           QFuture<void> future =
           QtConcurrent::run(  
           std::bind(  
           &Nav2Panel::onCancelButtonPressed,  
           this ) );
          }
          
          void
     507  Nav2Panel::onNewGoal(  double x,   double y,   double theta,   QString frame )
          {
           auto pose = geometry_msgs::msg::PoseStamped(   );
          
           pose.header.stamp = rclcpp::Clock(   ).now(   );
           pose.header.frame_id = frame.toStdString(   );
           pose.pose.position.x = x;
           pose.pose.position.y = y;
           pose.pose.position.z = 0.0;
           pose.pose.orientation = orientationAroundZAxis(  theta );
          
           if (  state_machine_.configuration(   ).contains(  accumulating_ ) ) {
           acummulated_poses_.push_back(  pose );
           } else {
           std::cout << "Start navigation" << std::endl;
           startNavigation(  pose );
           }
          
           updateWpNavigationMarkers(   );
          }
          
          void
     529  Nav2Panel::onCancelButtonPressed(   )
          {
           if (  navigation_goal_handle_ ) {
           auto future_cancel = navigation_action_client_->async_cancel_goal(  navigation_goal_handle_ );
          
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_cancel,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Failed to cancel goal" );
           } else {
           navigation_goal_handle_.reset(   );
           }
           }
          
           if (  waypoint_follower_goal_handle_ ) {
           auto future_cancel =
           waypoint_follower_action_client_->async_cancel_goal(  waypoint_follower_goal_handle_ );
          
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_cancel,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Failed to cancel waypoint follower" );
           } else {
           waypoint_follower_goal_handle_.reset(   );
           }
           }
          
           if (  nav_through_poses_goal_handle_ ) {
           auto future_cancel =
           nav_through_poses_action_client_->async_cancel_goal(  nav_through_poses_goal_handle_ );
          
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_cancel,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Failed to cancel nav through pose action" );
           } else {
           nav_through_poses_goal_handle_.reset(   );
           }
           }
          
          
           timer_.stop(   );
          }
          
          void
     574  Nav2Panel::onAccumulatedWp(   )
          {
           std::cout << "Start waypoint" << std::endl;
           startWaypointFollowing(  acummulated_poses_ );
           acummulated_poses_.clear(   );
          }
          
          void
     582  Nav2Panel::onAccumulatedNTP(   )
          {
           std::cout << "Start navigate through poses" << std::endl;
           startNavThroughPoses(  acummulated_poses_ );
           acummulated_poses_.clear(   );
          }
          
          void
     590  Nav2Panel::onAccumulating(   )
          {
           acummulated_poses_.clear(   );
          }
          
          void
     596  Nav2Panel::timerEvent(  QTimerEvent * event )
          {
           if (  state_machine_.configuration(   ).contains(  accumulated_wp_ ) ) {
           if (  event->timerId(   ) == timer_.timerId(   ) ) {
           if (  !waypoint_follower_goal_handle_ ) {
           RCLCPP_DEBUG(  client_node_->get_logger(   ),   "Waiting for Goal" );
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           return;
           }
          
           rclcpp::spin_some(  client_node_ );
           auto status = waypoint_follower_goal_handle_->get_status(   );
          
           // Check if the goal is still executing
           if (  status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
           {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::ACTIVE ) );
           } else {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           timer_.stop(   );
           }
           }
           } else if (  state_machine_.configuration(   ).contains(  accumulated_nav_through_poses_ ) ) {
           if (  event->timerId(   ) == timer_.timerId(   ) ) {
           if (  !nav_through_poses_goal_handle_ ) {
           RCLCPP_DEBUG(  client_node_->get_logger(   ),   "Waiting for Goal" );
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           return;
           }
          
           rclcpp::spin_some(  client_node_ );
           auto status = nav_through_poses_goal_handle_->get_status(   );
          
           // Check if the goal is still executing
           if (  status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
           {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::ACTIVE ) );
           } else {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           timer_.stop(   );
           }
           }
           } else {
           if (  event->timerId(   ) == timer_.timerId(   ) ) {
           if (  !navigation_goal_handle_ ) {
           RCLCPP_DEBUG(  client_node_->get_logger(   ),   "Waiting for Goal" );
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           return;
           }
          
           rclcpp::spin_some(  client_node_ );
           auto status = navigation_goal_handle_->get_status(   );
          
           // Check if the goal is still executing
           if (  status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
           {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::ACTIVE ) );
           } else {
           state_machine_.postEvent(  new ROSActionQEvent(  QActionState::INACTIVE ) );
           timer_.stop(   );
           }
           }
           }
          }
          
          void
     665  Nav2Panel::startWaypointFollowing(  std::vector<geometry_msgs::msg::PoseStamped> poses )
          {
           auto is_action_server_ready =
           waypoint_follower_action_client_->wait_for_action_server(  std::chrono::seconds(  5 ) );
           if (  !is_action_server_ready ) {
           RCLCPP_ERROR(  
           client_node_->get_logger(   ),   "follow_waypoints action server is not available."
           " Is the initial pose set?" );
           return;
           }
          
           // Send the goal poses
           waypoint_follower_goal_.poses = poses;
          
           RCLCPP_DEBUG(  
           client_node_->get_logger(   ),   "Sending a path of %zu waypoints:",  
           waypoint_follower_goal_.poses.size(   ) );
           for (  auto waypoint : waypoint_follower_goal_.poses ) {
           RCLCPP_DEBUG(  
           client_node_->get_logger(   ),  
           "\t(  %lf,   %lf )",   waypoint.pose.position.x,   waypoint.pose.position.y );
           }
          
           // Enable result awareness by providing an empty lambda function
           auto send_goal_options =
           rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SendGoalOptions(   );
           send_goal_options.result_callback = [this](  auto ) {
           waypoint_follower_goal_handle_.reset(   );
           };
          
           auto future_goal_handle =
           waypoint_follower_action_client_->async_send_goal(  waypoint_follower_goal_,   send_goal_options );
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_goal_handle,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Send goal call failed" );
           return;
           }
          
           // Get the goal handle and save so that we can check on completion in the timer callback
           waypoint_follower_goal_handle_ = future_goal_handle.get(   );
           if (  !waypoint_follower_goal_handle_ ) {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Goal was rejected by server" );
           return;
           }
          
           timer_.start(  200,   this );
          }
          
          void
     715  Nav2Panel::startNavThroughPoses(  std::vector<geometry_msgs::msg::PoseStamped> poses )
          {
           auto is_action_server_ready =
           nav_through_poses_action_client_->wait_for_action_server(  std::chrono::seconds(  5 ) );
           if (  !is_action_server_ready ) {
           RCLCPP_ERROR(  
           client_node_->get_logger(   ),   "navigate_through_poses action server is not available."
           " Is the initial pose set?" );
           return;
           }
          
           nav_through_poses_goal_.poses = poses;
           RCLCPP_INFO(  
           client_node_->get_logger(   ),  
           "NavigateThroughPoses will be called using the BT Navigator's default behavior tree." );
          
           RCLCPP_DEBUG(  
           client_node_->get_logger(   ),   "Sending a path of %zu waypoints:",  
           nav_through_poses_goal_.poses.size(   ) );
           for (  auto waypoint : nav_through_poses_goal_.poses ) {
           RCLCPP_DEBUG(  
           client_node_->get_logger(   ),  
           "\t(  %lf,   %lf )",   waypoint.pose.position.x,   waypoint.pose.position.y );
           }
          
           // Enable result awareness by providing an empty lambda function
           auto send_goal_options =
           rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SendGoalOptions(   );
           send_goal_options.result_callback = [this](  auto ) {
           nav_through_poses_goal_handle_.reset(   );
           };
          
           auto future_goal_handle =
           nav_through_poses_action_client_->async_send_goal(  nav_through_poses_goal_,   send_goal_options );
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_goal_handle,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Send goal call failed" );
           return;
           }
          
           // Get the goal handle and save so that we can check on completion in the timer callback
           nav_through_poses_goal_handle_ = future_goal_handle.get(   );
           if (  !nav_through_poses_goal_handle_ ) {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Goal was rejected by server" );
           return;
           }
          
           timer_.start(  200,   this );
          }
          
          void
     767  Nav2Panel::startNavigation(  geometry_msgs::msg::PoseStamped pose )
          {
           auto is_action_server_ready =
           navigation_action_client_->wait_for_action_server(  std::chrono::seconds(  5 ) );
           if (  !is_action_server_ready ) {
           RCLCPP_ERROR(  
           client_node_->get_logger(   ),  
           "navigate_to_pose action server is not available."
           " Is the initial pose set?" );
           return;
           }
          
           // Send the goal pose
           navigation_goal_.pose = pose;
          
           RCLCPP_INFO(  
           client_node_->get_logger(   ),  
           "NavigateToPose will be called using the BT Navigator's default behavior tree." );
          
           // Enable result awareness by providing an empty lambda function
           auto send_goal_options =
           rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions(   );
           send_goal_options.result_callback = [this](  auto ) {
           navigation_goal_handle_.reset(   );
           };
          
           auto future_goal_handle =
           navigation_action_client_->async_send_goal(  navigation_goal_,   send_goal_options );
           if (  rclcpp::spin_until_future_complete(  client_node_,   future_goal_handle,   server_timeout_ ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Send goal call failed" );
           return;
           }
          
           // Get the goal handle and save so that we can check on completion in the timer callback
           navigation_goal_handle_ = future_goal_handle.get(   );
           if (  !navigation_goal_handle_ ) {
           RCLCPP_ERROR(  client_node_->get_logger(   ),   "Goal was rejected by server" );
           return;
           }
          
           timer_.start(  200,   this );
          }
          
          void
     813  Nav2Panel::save(  rviz_common::Config config ) const
          {
           Panel::save(  config );
          }
          
          void
     819  Nav2Panel::load(  const rviz_common::Config & config )
          {
           Panel::load(  config );
          }
          
          void
     825  Nav2Panel::resetUniqueId(   )
          {
           unique_id = 0;
          }
          
          int
     831  Nav2Panel::getUniqueId(   )
          {
           int temp_id = unique_id;
           unique_id += 1;
           return temp_id;
          }
          
          void
     839  Nav2Panel::updateWpNavigationMarkers(   )
          {
           resetUniqueId(   );
          
           auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>(   );
          
           for (  size_t i = 0; i < acummulated_poses_.size(   ); i++ ) {
           // Draw a green arrow at the waypoint pose
           visualization_msgs::msg::Marker arrow_marker;
           arrow_marker.header = acummulated_poses_[i].header;
           arrow_marker.id = getUniqueId(   );
           arrow_marker.type = visualization_msgs::msg::Marker::ARROW;
           arrow_marker.action = visualization_msgs::msg::Marker::ADD;
           arrow_marker.pose = acummulated_poses_[i].pose;
           arrow_marker.scale.x = 0.3;
           arrow_marker.scale.y = 0.05;
           arrow_marker.scale.z = 0.02;
           arrow_marker.color.r = 0;
           arrow_marker.color.g = 255;
           arrow_marker.color.b = 0;
           arrow_marker.color.a = 1.0f;
           arrow_marker.lifetime = rclcpp::Duration(  0s );
           arrow_marker.frame_locked = false;
           marker_array->markers.push_back(  arrow_marker );
          
           // Draw a red circle at the waypoint pose
           visualization_msgs::msg::Marker circle_marker;
           circle_marker.header = acummulated_poses_[i].header;
           circle_marker.id = getUniqueId(   );
           circle_marker.type = visualization_msgs::msg::Marker::SPHERE;
           circle_marker.action = visualization_msgs::msg::Marker::ADD;
           circle_marker.pose = acummulated_poses_[i].pose;
           circle_marker.scale.x = 0.05;
           circle_marker.scale.y = 0.05;
           circle_marker.scale.z = 0.05;
           circle_marker.color.r = 255;
           circle_marker.color.g = 0;
           circle_marker.color.b = 0;
           circle_marker.color.a = 1.0f;
           circle_marker.lifetime = rclcpp::Duration(  0s );
           circle_marker.frame_locked = false;
           marker_array->markers.push_back(  circle_marker );
          
           // Draw the waypoint number
           visualization_msgs::msg::Marker marker_text;
           marker_text.header = acummulated_poses_[i].header;
           marker_text.id = getUniqueId(   );
           marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
           marker_text.action = visualization_msgs::msg::Marker::ADD;
           marker_text.pose = acummulated_poses_[i].pose;
           marker_text.pose.position.z += 0.2; // draw it on top of the waypoint
           marker_text.scale.x = 0.07;
           marker_text.scale.y = 0.07;
           marker_text.scale.z = 0.07;
           marker_text.color.r = 0;
           marker_text.color.g = 255;
           marker_text.color.b = 0;
           marker_text.color.a = 1.0f;
           marker_text.lifetime = rclcpp::Duration(  0s );
           marker_text.frame_locked = false;
           marker_text.text = "wp_" + std::to_string(  i + 1 );
           marker_array->markers.push_back(  marker_text );
           }
          
           if (  marker_array->markers.empty(   ) ) {
           visualization_msgs::msg::Marker clear_all_marker;
           clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
           marker_array->markers.push_back(  clear_all_marker );
           }
          
           wp_navigation_markers_pub_->publish(  std::move(  marker_array ) );
          }
          
          inline QString
     913  Nav2Panel::getGoalStatusLabel(  int8_t status )
          {
           std::string status_str;
           switch (  status ) {
           case action_msgs::msg::GoalStatus::STATUS_EXECUTING:
           status_str = "<font color=green>active</color>";
           break;
          
           case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED:
           status_str = "<font color=green>reached</color>";
           break;
          
           case action_msgs::msg::GoalStatus::STATUS_CANCELED:
           status_str = "<font color=orange>canceled</color>";
           break;
          
           case action_msgs::msg::GoalStatus::STATUS_ABORTED:
           status_str = "<font color=red>aborted</color>";
           break;
          
           case action_msgs::msg::GoalStatus::STATUS_UNKNOWN:
           status_str = "unknown";
           break;
          
           default:
           status_str = "inactive";
           break;
           }
           return QString(  
           std::string(  
           "<table><tr><td width=100><b>Feedback:</b></td><td>" +
           status_str + "</td></tr></table>" ).c_str(   ) );
          }
          
          inline QString
     948  Nav2Panel::getNavToPoseFeedbackLabel(  nav2_msgs::action::NavigateToPose::Feedback msg )
          {
           return QString(  std::string(  "<table>" + toLabel(  msg ) + "</table>" ).c_str(   ) );
          }
          
          inline QString
     954  Nav2Panel::getNavThroughPosesFeedbackLabel(  nav2_msgs::action::NavigateThroughPoses::Feedback msg )
          {
           return QString(  
           std::string(  
           "<table><tr><td width=150>Poses remaining:</td><td>" +
           std::to_string(  msg.number_of_poses_remaining ) +
           "</td></tr>" + toLabel(  msg ) + "</table>" ).c_str(   ) );
          }
          
          template<typename T>
     964  inline std::string Nav2Panel::toLabel(  T & msg )
          {
           return std::string(  
           "<tr><td width=150>ETA:</td><td>" +
           toString(  rclcpp::Duration(  msg.estimated_time_remaining ).seconds(   ),   0 ) + " s"
           "</td></tr><tr><td width=150>Distance remaining:</td><td>" +
           toString(  msg.distance_remaining,   2 ) + " m"
           "</td></tr><tr><td width=150>Time taken:</td><td>" +
           toString(  rclcpp::Duration(  msg.navigation_time ).seconds(   ),   0 ) + " s"
           "</td></tr><tr><td width=150>Recoveries:</td><td>" +
           std::to_string(  msg.number_of_recoveries ) +
           "</td></tr>" );
          }
          
          inline std::string
     979  Nav2Panel::toString(  double val,   int precision )
          {
           std::ostringstream out;
           out.precision(  precision );
           out << std::fixed << val;
           return out.str(   );
          }
          
          } // namespace nav2_rviz_plugins
          
          #include <pluginlib/class_list_macros.hpp> // NOLINT
     990  PLUGINLIB_EXPORT_CLASS(  nav2_rviz_plugins::Nav2Panel,   rviz_common::Panel )

navigation2/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp

       1  /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * Copyright (  c ) 2018,   Bosch Software Innovations GmbH.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
          
          #include <vector>
          #include <string>
          #include <algorithm>
          
          #include <OgreSceneManager.h>
          #include <OgreTechnique.h>
          
          #include "rviz_rendering/material_manager.hpp"
          
          namespace nav2_rviz_plugins
          {
          
      60  FlatWeightedArrowsArray::FlatWeightedArrowsArray(  Ogre::SceneManager * scene_manager )
          : scene_manager_(  scene_manager ),   manual_object_(  nullptr ) {}
          
      63  FlatWeightedArrowsArray::~FlatWeightedArrowsArray(   )
          {
           if (  manual_object_ ) {
           scene_manager_->destroyManualObject(  manual_object_ );
           }
          }
          
      70  void FlatWeightedArrowsArray::createAndAttachManualObject(  Ogre::SceneNode * scene_node )
          {
           manual_object_ = scene_manager_->createManualObject(   );
           manual_object_->setDynamic(  true );
           scene_node->attachObject(  manual_object_ );
          }
          
      77  void FlatWeightedArrowsArray::updateManualObject(  
      78   Ogre::ColourValue color,  
           float alpha,  
           float min_length,  
           float max_length,  
      82   const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses )
          {
           clear(   );
          
           color.a = alpha;
           setManualObjectMaterial(   );
           rviz_rendering::MaterialManager::enableAlphaBlending(  material_,   alpha );
          
           manual_object_->begin(  
           material_->getName(   ),   Ogre::RenderOperation::OT_LINE_LIST,   "rviz_rendering" );
           setManualObjectVertices(  color,   min_length,   max_length,   poses );
           manual_object_->end(   );
          }
          
      96  void FlatWeightedArrowsArray::clear(   )
          {
           if (  manual_object_ ) {
           manual_object_->clear(   );
           }
          }
          
     103  void FlatWeightedArrowsArray::setManualObjectMaterial(   )
          {
           static int material_count = 0;
           std::string material_name = "FlatWeightedArrowsMaterial" + std::to_string(  material_count++ );
           material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(  material_name );
          }
          
     110  void FlatWeightedArrowsArray::setManualObjectVertices(  
     111   const Ogre::ColourValue & color,  
           float min_length,  
           float max_length,  
     114   const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses )
          {
           manual_object_->estimateVertexCount(  poses.size(   ) * 6 );
          
           float scale = max_length - min_length;
           float length;
           for (  const auto & pose : poses ) {
           length = std::min(  std::max(  pose.weight * scale + min_length,   min_length ),   max_length );
           Ogre::Vector3 vertices[6];
           vertices[0] = pose.position; // back of arrow
           vertices[1] =
           pose.position + pose.orientation * Ogre::Vector3(  length,   0,   0 ); // tip of arrow
           vertices[2] = vertices[1];
           vertices[3] = pose.position + pose.orientation * Ogre::Vector3(  
           0.75f * length,   0.2f * length,   0 );
           vertices[4] = vertices[1];
           vertices[5] = pose.position + pose.orientation * Ogre::Vector3(  
           0.75f * length,   -0.2f * length,  
           0 );
          
           for (  const auto & vertex : vertices ) {
           manual_object_->position(  vertex );
           manual_object_->colour(  color );
           }
           }
          }
          
          } // namespace nav2_rviz_plugins

navigation2/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp

          /*
           * Copyright (  c ) 2012,   Willow Garage,   Inc.
           * Copyright (  c ) 2018,   Bosch Software Innovations GmbH.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above copyright
           * notice,   this list of conditions and the following disclaimer in the
           * documentation and/or other materials provided with the distribution.
           * * Neither the name of the Willow Garage,   Inc. nor the names of its
           * contributors may be used to endorse or promote products derived from
           * this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
           * AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
           * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
           * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
           * LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
           * CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
           * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
           * INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
           * CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
           * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           */
          
          // Copyright (  c ) 2019 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
          
          #include <memory>
          #include <string>
          
          #include <OgreManualObject.h>
          #include <OgreMaterialManager.h>
          #include <OgreTechnique.h>
          
          #include "rviz_common/logging.hpp"
          #include "rviz_common/msg_conversions.hpp"
          #include "rviz_common/properties/enum_property.hpp"
          #include "rviz_common/properties/color_property.hpp"
          #include "rviz_common/properties/float_property.hpp"
          #include "rviz_common/validate_floats.hpp"
          
          #include "rviz_rendering/objects/arrow.hpp"
          #include "rviz_rendering/objects/axes.hpp"
          
          #include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
          
          namespace nav2_rviz_plugins
          {
          namespace
          {
          struct ShapeType
          {
           enum
           {
           Arrow2d,  
           Arrow3d,  
           Axes,  
           };
          };
          
          } // namespace
          
      83  ParticleCloudDisplay::ParticleCloudDisplay(  
      84   rviz_common::DisplayContext * display_context,  
      85   Ogre::SceneNode * scene_node )
          : ParticleCloudDisplay(   )
          {
           context_ = display_context;
           scene_node_ = scene_node;
           scene_manager_ = context_->getSceneManager(   );
          
           arrows2d_ = std::make_unique<FlatWeightedArrowsArray>(  scene_manager_ );
           arrows2d_->createAndAttachManualObject(  scene_node );
           arrow_node_ = scene_node_->createChildSceneNode(   );
           axes_node_ = scene_node_->createChildSceneNode(   );
           updateShapeChoice(   );
          }
          
      99  ParticleCloudDisplay::ParticleCloudDisplay(   )
     100  : min_length_(  0.02f ),   max_length_(  0.3f )
          {
           initializeProperties(   );
          
           shape_property_->addOption(  "Arrow (  Flat )",   ShapeType::Arrow2d );
           shape_property_->addOption(  "Arrow (  3D )",   ShapeType::Arrow3d );
           shape_property_->addOption(  "Axes",   ShapeType::Axes );
           arrow_alpha_property_->setMin(  0 );
           arrow_alpha_property_->setMax(  1 );
           arrow_min_length_property_->setMax(  max_length_ );
           arrow_max_length_property_->setMin(  min_length_ );
          }
          
     113  void ParticleCloudDisplay::initializeProperties(   )
          {
           shape_property_ = new rviz_common::properties::EnumProperty(  
           "Shape",   "Arrow (  Flat )",   "Shape to display the pose as.",   this,   SLOT(  updateShapeChoice(   ) ) );
          
           arrow_color_property_ = new rviz_common::properties::ColorProperty(  
           "Color",   QColor(  255,   25,   0 ),   "Color to draw the arrows.",   this,   SLOT(  updateArrowColor(   ) ) );
          
           arrow_alpha_property_ = new rviz_common::properties::FloatProperty(  
           "Alpha",  
           1.0f,  
           "Amount of transparency to apply to the displayed poses.",  
           this,  
           SLOT(  updateArrowColor(   ) ) );
          
           arrow_min_length_property_ = new rviz_common::properties::FloatProperty(  
           "Min Arrow Length",   min_length_,   "Minimum length of the arrows.",   this,   SLOT(  updateGeometry(   ) ) );
          
           arrow_max_length_property_ = new rviz_common::properties::FloatProperty(  
           "Max Arrow Length",   max_length_,   "Maximum length of the arrows.",   this,   SLOT(  updateGeometry(   ) ) );
          
           // Scales are set based on initial values
           length_scale_ = max_length_ - min_length_;
           shaft_radius_scale_ = 0.0435;
           head_length_scale_ = 0.3043;
           head_radius_scale_ = 0.1304;
          }
          
     141  ParticleCloudDisplay::~ParticleCloudDisplay(   )
          {
           // because of forward declaration of arrow and axes,   destructor cannot be declared in .hpp as
           // default
          }
          
     147  void ParticleCloudDisplay::onInitialize(   )
          {
           MFDClass::onInitialize(   );
           arrows2d_ = std::make_unique<FlatWeightedArrowsArray>(  scene_manager_ );
           arrows2d_->createAndAttachManualObject(  scene_node_ );
           arrow_node_ = scene_node_->createChildSceneNode(   );
           axes_node_ = scene_node_->createChildSceneNode(   );
           updateShapeChoice(   );
          }
          
     157  void ParticleCloudDisplay::processMessage(  const nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg )
          {
           if (  !validateFloats(  *msg ) ) {
           setStatus(  
           rviz_common::properties::StatusProperty::Error,  
           "Topic",  
           "Message contained invalid floating point values (  nans or infs )" );
           return;
           }
          
           if (  !setTransform(  msg->header ) ) {
           return;
           }
          
           poses_.resize(  msg->particles.size(   ) );
          
           for (  std::size_t i = 0; i < msg->particles.size(   ); ++i ) {
           poses_[i].position = rviz_common::pointMsgToOgre(  msg->particles[i].pose.position );
           poses_[i].orientation = rviz_common::quaternionMsgToOgre(  msg->particles[i].pose.orientation );
           poses_[i].weight = static_cast<float>(  msg->particles[i].weight );
           }
          
           updateDisplay(   );
          
           context_->queueRender(   );
          }
          
     184  bool ParticleCloudDisplay::validateFloats(  const nav2_msgs::msg::ParticleCloud & msg )
          {
           for (  auto & particle : msg.particles ) {
           if (  !rviz_common::validateFloats(  particle.pose ) ||
           !rviz_common::validateFloats(  particle.weight ) )
           {
           return false;
           }
           }
           return true;
          }
          
     196  bool ParticleCloudDisplay::setTransform(  std_msgs::msg::Header const & header )
          {
           Ogre::Vector3 position;
           Ogre::Quaternion orientation;
           if (  !context_->getFrameManager(   )->getTransform(  header,   position,   orientation ) ) {
           setMissingTransformToFixedFrame(  header.frame_id );
           return false;
           }
           setTransformOk(   );
          
           scene_node_->setPosition(  position );
           scene_node_->setOrientation(  orientation );
           return true;
          }
          
     211  void ParticleCloudDisplay::updateDisplay(   )
          {
           int shape = shape_property_->getOptionInt(   );
           switch (  shape ) {
           case ShapeType::Arrow2d:
           updateArrows2d(   );
           arrows3d_.clear(   );
           axes_.clear(   );
           break;
           case ShapeType::Arrow3d:
           updateArrows3d(   );
           arrows2d_->clear(   );
           axes_.clear(   );
           break;
           case ShapeType::Axes:
           updateAxes(   );
           arrows2d_->clear(   );
           arrows3d_.clear(   );
           break;
           }
          }
          
     233  void ParticleCloudDisplay::updateArrows2d(   )
          {
           arrows2d_->updateManualObject(  
           arrow_color_property_->getOgreColor(   ),  
           arrow_alpha_property_->getFloat(   ),  
           min_length_,  
           max_length_,  
           poses_ );
          }
          
     243  void ParticleCloudDisplay::updateArrows3d(   )
          {
           while (  arrows3d_.size(   ) < poses_.size(   ) ) {
           arrows3d_.push_back(  makeArrow3d(   ) );
           }
           while (  arrows3d_.size(   ) > poses_.size(   ) ) {
           arrows3d_.pop_back(   );
           }
          
           Ogre::Quaternion adjust_orientation(  Ogre::Degree(  -90 ),   Ogre::Vector3::UNIT_Y );
           float shaft_length;
           for (  std::size_t i = 0; i < poses_.size(   ); ++i ) {
           shaft_length = std::min(  
           std::max(  
           poses_[i].weight * length_scale_ + min_length_,  
           min_length_ ),   max_length_ );
           arrows3d_[i]->set(  
           shaft_length,  
           shaft_length * shaft_radius_scale_,  
           shaft_length * head_length_scale_,  
           shaft_length * head_radius_scale_
            );
           arrows3d_[i]->setPosition(  poses_[i].position );
           arrows3d_[i]->setOrientation(  poses_[i].orientation * adjust_orientation );
           }
          }
          
     270  void ParticleCloudDisplay::updateAxes(   )
          {
           while (  axes_.size(   ) < poses_.size(   ) ) {
           axes_.push_back(  makeAxes(   ) );
           }
           while (  axes_.size(   ) > poses_.size(   ) ) {
           axes_.pop_back(   );
           }
           float shaft_length;
           for (  std::size_t i = 0; i < poses_.size(   ); ++i ) {
           shaft_length = std::min(  
           std::max(  
           poses_[i].weight * length_scale_ + min_length_,  
           min_length_ ),   max_length_ );
           axes_[i]->set(  shaft_length,   shaft_length * shaft_radius_scale_ );
           axes_[i]->setPosition(  poses_[i].position );
           axes_[i]->setOrientation(  poses_[i].orientation );
           }
          }
          
     290  std::unique_ptr<rviz_rendering::Arrow> ParticleCloudDisplay::makeArrow3d(   )
          {
           Ogre::ColourValue color = arrow_color_property_->getOgreColor(   );
           color.a = arrow_alpha_property_->getFloat(   );
          
           auto arrow = std::make_unique<rviz_rendering::Arrow>(  
           scene_manager_,  
           arrow_node_,  
           min_length_,  
           min_length_ * shaft_radius_scale_,  
           min_length_ * head_length_scale_,  
           min_length_ * head_radius_scale_
            );
          
           arrow->setColor(  color );
           return arrow;
          }
          
     308  std::unique_ptr<rviz_rendering::Axes> ParticleCloudDisplay::makeAxes(   )
          {
           return std::make_unique<rviz_rendering::Axes>(  
           scene_manager_,  
           axes_node_,  
           min_length_,  
           min_length_ * shaft_radius_scale_
            );
          }
          
     318  void ParticleCloudDisplay::reset(   )
          {
           MFDClass::reset(   );
           arrows2d_->clear(   );
           arrows3d_.clear(   );
           axes_.clear(   );
          }
          
     326  void ParticleCloudDisplay::updateShapeChoice(   )
          {
           int shape = shape_property_->getOptionInt(   );
           bool use_axes = shape == ShapeType::Axes;
          
           arrow_color_property_->setHidden(  use_axes );
           arrow_alpha_property_->setHidden(  use_axes );
          
           if (  initialized(   ) ) {
           updateDisplay(   );
           }
          }
          
     339  void ParticleCloudDisplay::updateArrowColor(   )
          {
           int shape = shape_property_->getOptionInt(   );
           Ogre::ColourValue color = arrow_color_property_->getOgreColor(   );
           color.a = arrow_alpha_property_->getFloat(   );
          
           if (  shape == ShapeType::Arrow2d ) {
           updateArrows2d(   );
           } else if (  shape == ShapeType::Arrow3d ) {
           for (  const auto & arrow : arrows3d_ ) {
           arrow->setColor(  color );
           }
           }
           context_->queueRender(   );
          }
          
     355  void ParticleCloudDisplay::updateGeometry(   )
          {
           min_length_ = arrow_min_length_property_->getFloat(   );
           max_length_ = arrow_max_length_property_->getFloat(   );
           length_scale_ = max_length_ - min_length_;
          
           arrow_min_length_property_->setMax(  max_length_ );
           arrow_max_length_property_->setMin(  min_length_ );
          
           int shape = shape_property_->getOptionInt(   );
           switch (  shape ) {
           case ShapeType::Arrow2d:
           updateArrows2d(   );
           arrows3d_.clear(   );
           axes_.clear(   );
           break;
           case ShapeType::Arrow3d:
           updateArrow3dGeometry(   );
           arrows2d_->clear(   );
           axes_.clear(   );
           break;
           case ShapeType::Axes:
           updateAxesGeometry(   );
           arrows2d_->clear(   );
           arrows3d_.clear(   );
           break;
           }
          
           context_->queueRender(   );
          }
          
     386  void ParticleCloudDisplay::updateArrow3dGeometry(   )
          {
           float shaft_length;
           for (  std::size_t i = 0; i < poses_.size(   ) && i < arrows3d_.size(   ); ++i ) {
           shaft_length = std::min(  
           std::max(  
           poses_[i].weight * length_scale_ + min_length_,  
           min_length_ ),   max_length_ );
           arrows3d_[i]->set(  
           shaft_length,  
           shaft_length * shaft_radius_scale_,  
           shaft_length * head_length_scale_,  
           shaft_length * head_radius_scale_
            );
           }
          }
          
     403  void ParticleCloudDisplay::updateAxesGeometry(   )
          {
           float shaft_length;
           for (  std::size_t i = 0; i < poses_.size(   ) && i < axes_.size(   ); ++i ) {
           shaft_length = std::min(  
           std::max(  
           poses_[i].weight * length_scale_ + min_length_,  
           min_length_ ),   max_length_ );
           axes_[i]->set(  shaft_length,   shaft_length * shaft_radius_scale_ );
           }
          }
          
     415  void ParticleCloudDisplay::setShape(  QString shape )
          {
           shape_property_->setValue(  shape );
          }
          
          } // namespace nav2_rviz_plugins
          
          #include <pluginlib/class_list_macros.hpp> // NOLINT
          PLUGINLIB_EXPORT_CLASS(  nav2_rviz_plugins::ParticleCloudDisplay,   rviz_common::Display )

navigation2/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          // Copyright (  c ) 2020,   Applied Electric Vehicles Pty Ltd
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
          #define NAV2_SMAC_PLANNER__A_STAR_HPP_
          
          #include <vector>
          #include <iostream>
          #include <unordered_map>
          #include <memory>
          #include <queue>
          #include <utility>
          #include "Eigen/Core"
          
          #include "nav2_costmap_2d/costmap_2d.hpp"
          
          #include "nav2_smac_planner/analytic_expansion.hpp"
          #include "nav2_smac_planner/node_2d.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "nav2_smac_planner/node_basic.hpp"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/constants.hpp"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::AStarAlgorithm
           * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
           */
          template<typename NodeT>
      45  class AStarAlgorithm
          {
          public:
           typedef NodeT * NodePtr;
           typedef std::unordered_map<unsigned int,   NodeT> Graph;
           typedef std::vector<NodePtr> NodeVector;
           typedef std::pair<float,   NodeBasic<NodeT>> NodeElement;
           typedef typename NodeT::Coordinates Coordinates;
           typedef typename NodeT::CoordinateVector CoordinateVector;
           typedef typename NodeVector::iterator NeighborIterator;
           typedef std::function<bool (  const unsigned int &,   NodeT * & )> NodeGetter;
          
           /**
           * @struct nav2_smac_planner::NodeComparator
           * @brief Node comparison for priority queue sorting
           */
           struct NodeComparator
           {
           bool operator(   )(  const NodeElement & a,   const NodeElement & b ) const
           {
           return a.first > b.first;
           }
           };
          
           typedef std::priority_queue<NodeElement,   std::vector<NodeElement>,   NodeComparator> NodeQueue;
          
           /**
           * @brief A constructor for nav2_smac_planner::PlannerServer
           * @param neighborhood The type of neighborhood to use for search (  4 or 8 connected )
           */
      75   explicit AStarAlgorithm(  const MotionModel & motion_model,   const SearchInfo & search_info );
          
           /**
           * @brief A destructor for nav2_smac_planner::AStarAlgorithm
           */
      80   ~AStarAlgorithm(   );
          
           /**
           * @brief Initialization of the planner with defaults
           * @param allow_unknown Allow search in unknown space,   good for navigation while mapping
           * @param max_iterations Maximum number of iterations to use while expanding search
           * @param max_on_approach_iterations Maximum number of iterations before returning a valid
           * path once within thresholds to refine path
           * comes at more compute time but smoother paths.
           * @param max_planning_time Maximum time (  in seconds ) to wait for a plan,   createPath returns
           * false after this timeout
           */
      92   void initialize(  
           const bool & allow_unknown,  
           int & max_iterations,  
           const int & max_on_approach_iterations,  
           const double & max_planning_time,  
           const float & lookup_table_size,  
           const unsigned int & dim_3_size );
          
           /**
           * @brief Creating path from given costmap,   start,   and goal
           * @param path Reference to a vector of indicies of generated path
           * @param num_iterations Reference to number of iterations to create plan
           * @param tolerance Reference to tolerance in costmap nodes
           * @return if plan was successful
           */
     107   bool createPath(  CoordinateVector & path,   int & num_iterations,   const float & tolerance );
          
           /**
           * @brief Sets the collision checker to use
           * @param collision_checker Collision checker to use for checking state validity
           */
     113   void setCollisionChecker(  GridCollisionChecker * collision_checker );
          
           /**
           * @brief Set the goal for planning,   as a node index
           * @param mx The node X index of the goal
           * @param my The node Y index of the goal
           * @param dim_3 The node dim_3 index of the goal
           */
     121   void setGoal(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 );
          
           /**
           * @brief Set the starting pose for planning,   as a node index
           * @param mx The node X index of the goal
           * @param my The node Y index of the goal
           * @param dim_3 The node dim_3 index of the goal
           */
     132   void setStart(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 );
          
           /**
           * @brief Get maximum number of iterations to plan
           * @return Reference to Maximum iterations parameter
           */
     141   int & getMaxIterations(   );
          
           /**
           * @brief Get pointer reference to starting node
           * @return Node pointer reference to starting node
           */
     147   NodePtr & getStart(   );
          
           /**
           * @brief Get pointer reference to goal node
           * @return Node pointer reference to goal node
           */
     153   NodePtr & getGoal(   );
          
           /**
           * @brief Get maximum number of on-approach iterations after within threshold
           * @return Reference to Maximum on-appraoch iterations parameter
           */
     159   int & getOnApproachMaxIterations(   );
          
           /**
           * @brief Get tolerance,   in node nodes
           * @return Reference to tolerance parameter
           */
     165   float & getToleranceHeuristic(   );
          
           /**
           * @brief Get size of graph in X
           * @return Size in X
           */
     171   unsigned int & getSizeX(   );
          
           /**
           * @brief Get size of graph in Y
           * @return Size in Y
           */
     177   unsigned int & getSizeY(   );
          
           /**
           * @brief Get number of angle quantization bins (  SE2 ) or Z coordinate (  XYZ )
           * @return Number of angle bins / Z dimension
           */
     183   unsigned int & getSizeDim3(   );
          
          protected:
           /**
           * @brief Get pointer to next goal in open set
           * @return Node pointer reference to next heuristically scored node
           */
     190   inline NodePtr getNextNode(   );
          
           /**
           * @brief Get pointer to next goal in open set
           * @param cost The cost to sort into the open set of the node
           * @param node Node pointer reference to add to open set
           */
     197   inline void addNode(  const float & cost,   NodePtr & node );
          
           /**
           * @brief Adds node to graph
           * @param cost The cost to sort into the open set of the node
           * @param node Node pointer reference to add to open set
           */
     204   inline NodePtr addToGraph(  const unsigned int & index );
          
           /**
           * @brief Check if this node is the goal node
           * @param node Node pointer to check if its the goal node
           * @return if node is goal
           */
     211   inline bool isGoal(  NodePtr & node );
          
           /**
           * @brief Get cost of heuristic of node
           * @param node Node index current
           * @param node Node index of new
           * @return Heuristic cost between the nodes
           */
     219   inline float getHeuristicCost(  const NodePtr & node );
          
           /**
           * @brief Check if inputs to planner are valid
           * @return Are valid
           */
     225   inline bool areInputsValid(   );
          
           /**
           * @brief Clear hueristic queue of nodes to search
           */
     230   inline void clearQueue(   );
          
           /**
           * @brief Clear graph of nodes searched
           */
     235   inline void clearGraph(   );
          
           int _timing_interval = 5000;
          
           bool _traverse_unknown;
           int _max_iterations;
           int _max_on_approach_iterations;
           double _max_planning_time;
           float _tolerance;
           unsigned int _x_size;
           unsigned int _y_size;
           unsigned int _dim3_size;
           SearchInfo _search_info;
          
           Coordinates _goal_coordinates;
           NodePtr _start;
           NodePtr _goal;
          
           Graph _graph;
           NodeQueue _queue;
          
           MotionModel _motion_model;
           NodeHeuristicPair _best_heuristic_node;
          
           GridCollisionChecker * _collision_checker;
           nav2_costmap_2d::Costmap2D * _costmap;
     261   std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__A_STAR_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

          // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
          #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
          
          #include <string>
          #include <vector>
          #include <list>
          #include <memory>
          
          #include "nav2_smac_planner/node_2d.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/constants.hpp"
          
          namespace nav2_smac_planner
          {
          
          template<typename NodeT>
      33  class AnalyticExpansion
          {
          public:
           typedef NodeT * NodePtr;
           typedef typename NodeT::Coordinates Coordinates;
           typedef std::function<bool (  const unsigned int &,   NodeT * & )> NodeGetter;
          
           /**
           * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
           * @brief Analytic expansion nodes and associated metadata
           */
           struct AnalyticExpansionNode
           {
           AnalyticExpansionNode(  
           NodePtr & node_in,  
           Coordinates & initial_coords_in,  
           Coordinates & proposed_coords_in )
           : node(  node_in ),  
           initial_coords(  initial_coords_in ),  
           proposed_coords(  proposed_coords_in )
           {
           }
          
           NodePtr node;
           Coordinates initial_coords;
           Coordinates proposed_coords;
           };
          
           typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
          
           /**
           * @brief Constructor for analytic expansion object
           */
      66   AnalyticExpansion(  
           const MotionModel & motion_model,  
           const SearchInfo & search_info,  
           const bool & traverse_unknown,  
           const unsigned int & dim_3_size );
          
           /**
           * @brief Sets the collision checker and costmap to use in expansion validation
           * @param collision_checker Collision checker to use
           */
      76   void setCollisionChecker(  GridCollisionChecker * collision_checker );
          
           /**
           * @brief Attempt an analytic path completion
           * @param node The node to start the analytic path from
           * @param goal The goal node to plan to
           * @param getter Gets a node at a set of coordinates
           * @param iterations Iterations to run over
           * @param best_cost Best heuristic cost to propertionally expand more closer to the goal
           * @return Node pointer reference to goal node if successful,   else
           * return nullptr
           */
      88   NodePtr tryAnalyticExpansion(  
           const NodePtr & current_node,  
           const NodePtr & goal_node,  
           const NodeGetter & getter,   int & iterations,   int & best_cost );
          
           /**
           * @brief Perform an analytic path expansion to the goal
           * @param node The node to start the analytic path from
           * @param goal The goal node to plan to
           * @param getter The function object that gets valid nodes from the graph
           * @return A set of analytically expanded nodes to the goal from current node,   if possible
           */
     100   AnalyticExpansionNodes getAnalyticPath(  
           const NodePtr & node,   const NodePtr & goal,  
           const NodeGetter & getter );
          
           /**
           * @brief Takes final analytic expansion and appends to current expanded node
           * @param node The node to start the analytic path from
           * @param goal The goal node to plan to
           * @param expanded_nodes Expanded nodes to append to end of current search path
           * @return Node pointer to goal node if successful,   else return nullptr
           */
     111   NodePtr setAnalyticPath(  
           const NodePtr & node,   const NodePtr & goal,  
           const AnalyticExpansionNodes & expanded_nodes );
          
           /**
           * @brief Takes an expanded nodes to clean up,   if necessary,   of any state
           * information that may be poluting it from a prior search iteration
           * @param expanded_nodes Expanded node to clean up from search
           */
     120   void cleanNode(  const NodePtr & nodes );
          
          protected:
           MotionModel _motion_model;
           SearchInfo _search_info;
           bool _traverse_unknown;
           unsigned int _dim_3_size;
           GridCollisionChecker * _collision_checker;
           std::list<std::unique_ptr<NodeT>> _detached_nodes;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          #include <vector>
          #include "nav2_costmap_2d/footprint_collision_checker.hpp"
          #include "nav2_smac_planner/constants.hpp"
          
          #ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
          #define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::GridCollisionChecker
           * @brief A costmap grid collision checker
           */
      28  class GridCollisionChecker
      29   : public nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::GridCollisionChecker
           * for use when regular bin intervals are appropriate
           * @param costmap The costmap to collision check against
           * @param num_quantizations The number of quantizations to precompute footprint
           * orientations for to speed up collision checking
           */
      39   GridCollisionChecker(  
      40   nav2_costmap_2d::Costmap2D * costmap,  
           unsigned int num_quantizations );
          
           /**
           * @brief A constructor for nav2_smac_planner::GridCollisionChecker
           * for use when irregular bin intervals are appropriate
           * @param costmap The costmap to collision check against
           * @param angles The vector of possible angle bins to precompute for
           * orientations for to speed up collision checking,   in radians
           */
           // GridCollisionChecker(  
           // nav2_costmap_2d::Costmap2D * costmap,  
           // std::vector<float> & angles );
          
           /**
           * @brief Set the footprint to use with collision checker
           * @param footprint The footprint to collision check against
           * @param radius Whether or not the footprint is a circle and use radius collision checking
           */
      59   void setFootprint(  
      60   const nav2_costmap_2d::Footprint & footprint,  
      61   const bool & radius,  
           const double & possible_inscribed_cost );
          
           /**
           * @brief Check if in collision with costmap and footprint at pose
           * @param x X coordinate of pose to check against
           * @param y Y coordinate of pose to check against
           * @param theta Angle bin number of pose to check against (  NOT radians )
           * @param traverse_unknown Whether or not to traverse in unknown space
           * @return boolean if in collision or not.
           */
      72   bool inCollision(  
           const float & x,  
           const float & y,  
           const float & theta,  
      76   const bool & traverse_unknown );
          
           /**
           * @brief Check if in collision with costmap and footprint at pose
           * @param i Index to search collision status of
           * @param traverse_unknown Whether or not to traverse in unknown space
           * @return boolean if in collision or not.
           */
      84   bool inCollision(  
           const unsigned int & i,  
      86   const bool & traverse_unknown );
          
           /**
           * @brief Get cost at footprint pose in costmap
           * @return the cost at the pose in costmap
           */
      92   float getCost(   );
          
           /**
           * @brief Get the angles of the precomputed footprint orientations
           * @return the ordered vector of angles corresponding to footprints
           */
      98   std::vector<float> & getPrecomputedAngles(   )
           {
           return angles_;
           }
          
          private:
           /**
           * @brief Check if value outside the range
           * @param min Minimum value of the range
           * @param max Maximum value of the range
           * @param value the value to check if it is within the range
           * @return boolean if in range or not
           */
     111   bool outsideRange(  const unsigned int & max,   const float & value );
          
          protected:
     114   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
     115   nav2_costmap_2d::Footprint unoriented_footprint_;
           double footprint_cost_;
     117   bool footprint_is_radius_;
     118   std::vector<float> angles_;
           double possible_inscribed_cost_{-1};
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/constants.hpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__CONSTANTS_HPP_
          #define NAV2_SMAC_PLANNER__CONSTANTS_HPP_
          
          #include <string>
          
          namespace nav2_smac_planner
          {
      22  enum class MotionModel
          {
           UNKNOWN = 0,  
           VON_NEUMANN = 1,  
           MOORE = 2,  
           DUBIN = 3,  
           REEDS_SHEPP = 4,  
           STATE_LATTICE = 5,  
          };
          
      32  inline std::string toString(  const MotionModel & n )
          {
           switch (  n ) {
           case MotionModel::VON_NEUMANN:
           return "Von Neumann";
           case MotionModel::MOORE:
           return "Moore";
           case MotionModel::DUBIN:
           return "Dubin";
           case MotionModel::REEDS_SHEPP:
           return "Reeds-Shepp";
           case MotionModel::STATE_LATTICE:
           return "State Lattice";
           default:
           return "Unknown";
           }
          }
          
      50  inline MotionModel fromString(  const std::string & n )
          {
           if (  n == "VON_NEUMANN" ) {
           return MotionModel::VON_NEUMANN;
           } else if (  n == "MOORE" ) {
           return MotionModel::MOORE;
           } else if (  n == "DUBIN" ) {
           return MotionModel::DUBIN;
           } else if (  n == "REEDS_SHEPP" ) {
           return MotionModel::REEDS_SHEPP;
           } else if (  n == "STATE_LATTICE" ) {
           return MotionModel::STATE_LATTICE;
           } else {
           return MotionModel::UNKNOWN;
           }
          }
          
          const float UNKNOWN = 255.0;
          const float OCCUPIED = 254.0;
          const float INSCRIBED = 253.0;
          const float MAX_NON_OBSTACLE = 252.0;
          const float FREE = 0;
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__CONSTANTS_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp

       1  // Copyright (  c ) 2020,   Carlos Luis
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
          #define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
          
          #include <algorithm>
          #include <string>
          #include <memory>
          
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_smac_planner/constants.hpp"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::CostmapDownsampler
           * @brief A costmap downsampler for more efficient path planning
           */
      32  class CostmapDownsampler
          {
          public:
           /**
           * @brief A constructor for CostmapDownsampler
           */
      38   CostmapDownsampler(   );
          
           /**
           * @brief A destructor for CostmapDownsampler
           */
      43   ~CostmapDownsampler(   );
          
           /**
           * @brief Configure the downsampled costmap object and the ROS publisher
           * @param node Lifecycle node pointer
           * @param global_frame The ID of the global frame used by the costmap
           * @param topic_name The name of the topic to publish the downsampled costmap
           * @param costmap The costmap we want to downsample
           * @param downsampling_factor Multiplier for the costmap resolution
           * @param use_min_cost_neighbor If true,   min function is used instead of max for downsampling
           */
      54   void on_configure(  
           const nav2_util::LifecycleNode::WeakPtr & node,  
           const std::string & global_frame,  
           const std::string & topic_name,  
           nav2_costmap_2d::Costmap2D * const costmap,  
           const unsigned int & downsampling_factor,  
           const bool & use_min_cost_neighbor = false );
          
           /**
           * @brief Activate the publisher of the downsampled costmap
           */
      65   void on_activate(   );
          
           /**
           * @brief Deactivate the publisher of the downsampled costmap
           */
      70   void on_deactivate(   );
          
           /**
           * @brief Cleanup the publisher of the downsampled costmap
           */
      75   void on_cleanup(   );
          
           /**
           * @brief Downsample the given costmap by the downsampling factor,   and publish the downsampled costmap
           * @param downsampling_factor Multiplier for the costmap resolution
           * @return A ptr to the downsampled costmap
           */
      82   nav2_costmap_2d::Costmap2D * downsample(  const unsigned int & downsampling_factor );
          
           /**
           * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version
           */
      87   void resizeCostmap(   );
          
          protected:
           /**
           * @brief Update the sizes X-Y of the costmap and its downsampled version
           */
      93   void updateCostmapSize(   );
          
           /**
           * @brief Explore all subcells of the original costmap and assign the max cost to the new (  downsampled ) cell
           * @param new_mx The X-coordinate of the cell in the new costmap
           * @param new_my The Y-coordinate of the cell in the new costmap
           */
     100   void setCostOfCell(  
           const unsigned int & new_mx,  
           const unsigned int & new_my );
          
           unsigned int _size_x;
           unsigned int _size_y;
           unsigned int _downsampled_size_x;
           unsigned int _downsampled_size_y;
           unsigned int _downsampling_factor;
     109   bool _use_min_cost_neighbor;
           float _downsampled_resolution;
     111   nav2_costmap_2d::Costmap2D * _costmap;
     112   std::unique_ptr<nav2_costmap_2d::Costmap2D> _downsampled_costmap;
     113   std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_
          #define NAV2_SMAC_PLANNER__NODE_2D_HPP_
          
          #include <math.h>
          #include <vector>
          #include <iostream>
          #include <memory>
          #include <queue>
          #include <limits>
          #include <utility>
          #include <functional>
          
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/constants.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::Node2D
           * @brief Node2D implementation for graph
           */
      39  class Node2D
          {
          public:
           typedef Node2D * NodePtr;
           typedef std::unique_ptr<std::vector<Node2D>> Graph;
           typedef std::vector<NodePtr> NodeVector;
          
           /**
           * @class nav2_smac_planner::Node2D::Coordinates
           * @brief Node2D implementation of coordinate structure
           */
           struct Coordinates
           {
           Coordinates(   ) {}
           Coordinates(  const float & x_in,   const float & y_in )
           : x(  x_in ),   y(  y_in )
           {}
          
           float x,   y;
           };
           typedef std::vector<Coordinates> CoordinateVector;
          
           /**
           * @brief A constructor for nav2_smac_planner::Node2D
           * @param index The index of this node for self-reference
           */
           explicit Node2D(  const unsigned int index );
          
           /**
           * @brief A destructor for nav2_smac_planner::Node2D
           */
           ~Node2D(   );
          
           /**
           * @brief operator== for comparisons
           * @param Node2D right hand side node reference
           * @return If cell indicies are equal
           */
           bool operator==(  const Node2D & rhs )
           {
           return this->_index == rhs._index;
           }
          
           /**
           * @brief Reset method for new search
           */
           void reset(   );
           /**
           * @brief Gets the accumulated cost at this node
           * @return accumulated cost
           */
           inline float & getAccumulatedCost(   )
           {
           return _accumulated_cost;
           }
          
           /**
           * @brief Sets the accumulated cost at this node
           * @param reference to accumulated cost
           */
           inline void setAccumulatedCost(  const float & cost_in )
           {
           _accumulated_cost = cost_in;
           }
          
           /**
           * @brief Gets the costmap cost at this node
           * @return costmap cost
           */
           inline float & getCost(   )
           {
           return _cell_cost;
           }
          
           /**
           * @brief Gets the costmap cost at this node
           * @return costmap cost
           */
           inline void setCost(  const float & cost )
           {
           _cell_cost = cost;
           }
          
           /**
           * @brief Gets if cell has been visited in search
           * @param If cell was visited
           */
           inline bool & wasVisited(   )
           {
           return _was_visited;
           }
          
           /**
           * @brief Sets if cell has been visited in search
           */
           inline void visited(   )
           {
           _was_visited = true;
           _is_queued = false;
           }
          
           /**
           * @brief Gets if cell is currently queued in search
           * @param If cell was queued
           */
           inline bool & isQueued(   )
           {
           return _is_queued;
           }
          
           /**
           * @brief Sets if cell is currently queued in search
           */
           inline void queued(   )
           {
           _is_queued = true;
           }
          
           /**
           * @brief Gets cell index
           * @return Reference to cell index
           */
           inline unsigned int & getIndex(   )
           {
           return _index;
           }
          
           /**
           * @brief Check if this node is valid
           * @param traverse_unknown If we can explore unknown nodes on the graph
           * @param collision_checker Pointer to collision checker object
           * @return whether this node is valid and collision free
           */
           bool isNodeValid(  const bool & traverse_unknown,   GridCollisionChecker * collision_checker );
          
           /**
           * @brief get traversal cost from this node to child node
           * @param child Node pointer to this node's child
           * @return traversal cost
           */
           float getTraversalCost(  const NodePtr & child );
          
           /**
           * @brief Get index
           * @param x x coordinate of point to get index of
           * @param y y coordinate of point to get index of
           * @param width width of costmap
           * @return index
           */
           static inline unsigned int getIndex(  
           const unsigned int & x,   const unsigned int & y,   const unsigned int & width )
           {
           return x + y * width;
           }
          
           /**
           * @brief Get index
           * @param Index Index of point
           * @param width width of costmap
           * @param angles angle bins to use (  must be 1 or throws exception )
           * @return coordinates of point
           */
           static inline Coordinates getCoords(  
           const unsigned int & index,   const unsigned int & width,   const unsigned int & angles )
           {
           if (  angles != 1 ) {
           throw std::runtime_error(  "Node type Node2D does not have a valid angle quantization." );
           }
          
           return Coordinates(  index % width,   index / width );
           }
          
           /**
           * @brief Get index
           * @param Index Index of point
           * @return coordinates of point
           */
           static inline Coordinates getCoords(  const unsigned int & index )
           {
           const unsigned int & size_x = _neighbors_grid_offsets[3];
           return Coordinates(  index % size_x,   index / size_x );
           }
          
           /**
           * @brief Get cost of heuristic of node
           * @param node Node index current
           * @param node Node index of new
           * @param costmap Costmap ptr to use
           * @return Heuristic cost between the nodes
           */
           static float getHeuristicCost(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coordinates,  
           const nav2_costmap_2d::Costmap2D * costmap );
          
           /**
           * @brief Initialize the neighborhood to be used in A*
           * We support 4-connect (  VON_NEUMANN ) and 8-connect (  MOORE )
           * @param neighborhood The desired neighborhood type
           * @param x_size_uint The total x size to find neighbors
           * @param y_size The total y size to find neighbors
           * @param num_angle_quantization Number of quantizations,   must be 0
           * @param search_info Search parameters,   unused by 2D node
           */
           static void initMotionModel(  
           const MotionModel & motion_model,  
           unsigned int & size_x,  
           unsigned int & size_y,  
           unsigned int & num_angle_quantization,  
           SearchInfo & search_info );
          
           /**
           * @brief Retrieve all valid neighbors of a node.
           * @param validity_checker Functor for state validity checking
           * @param collision_checker Collision checker to use
           * @param traverse_unknown If unknown costs are valid to traverse
           * @param neighbors Vector of neighbors to be filled
           */
           void getNeighbors(  
           std::function<bool(  const unsigned int &,   nav2_smac_planner::Node2D * & )> & validity_checker,  
           GridCollisionChecker * collision_checker,  
           const bool & traverse_unknown,  
           NodeVector & neighbors );
          
           /**
           * @brief Set the starting pose for planning,   as a node index
           * @param path Reference to a vector of indicies of generated path
           * @return whether the path was able to be backtraced
           */
           bool backtracePath(  CoordinateVector & path );
          
           Node2D * parent;
           static float cost_travel_multiplier;
           static std::vector<int> _neighbors_grid_offsets;
          
          private:
           float _cell_cost;
           float _accumulated_cost;
           unsigned int _index;
           bool _was_visited;
           bool _is_queued;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__NODE_2D_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
          #define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
          
          #include <math.h>
          #include <vector>
          #include <cmath>
          #include <iostream>
          #include <functional>
          #include <queue>
          #include <memory>
          #include <utility>
          #include <limits>
          
          #include "ompl/base/StateSpace.h"
          
          #include "nav2_smac_planner/constants.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "nav2_smac_planner/node_2d.hpp"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::NodeBasic
           * @brief NodeBasic implementation for priority queue insertion
           */
          template<typename NodeT>
      45  class NodeBasic
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::NodeBasic
           * @param cost_in The costmap cost at this node
           * @param index The index of this node for self-reference
           */
      53   explicit NodeBasic(  const unsigned int index )
           : index(  index ),  
           graph_node_ptr(  nullptr )
           {
           }
          
           /**
           * @brief Take a NodeBasic and populate it with any necessary state
           * cached in the queue for NodeT.
           * @param node NodeT ptr to populate metadata into NodeBasic
           */
      64   void populateSearchNode(  NodeT * & node );
          
           /**
           * @brief Take a NodeBasic and populate it with any necessary state
           * cached in the queue for NodeTs.
           * @param node Search node (  basic ) object to initialize internal node
           * with state
           */
      72   void processSearchNode(   );
          
           typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice
           NodeT * graph_node_ptr;
           MotionPrimitive * prim_ptr; // Used by NodeLattice
           unsigned int index,   motion_index;
           bool backward;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
          #define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
          
          #include <math.h>
          #include <vector>
          #include <cmath>
          #include <iostream>
          #include <functional>
          #include <queue>
          #include <memory>
          #include <utility>
          #include <limits>
          
          #include "ompl/base/StateSpace.h"
          
          #include "nav2_smac_planner/constants.hpp"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/costmap_downsampler.hpp"
          
          namespace nav2_smac_planner
          {
          
          typedef std::vector<float> LookupTable;
          typedef std::pair<double,   double> TrigValues;
          
          typedef std::pair<float,   unsigned int> ObstacleHeuristicElement;
          struct ObstacleHeuristicComparator
          {
           bool operator(   )(  const ObstacleHeuristicElement & a,   const ObstacleHeuristicElement & b ) const
           {
           return a.first > b.first;
           }
          };
          
          typedef std::vector<ObstacleHeuristicElement> ObstacleHeuristicQueue;
          
          // Must forward declare
      53  class NodeHybrid;
          
          /**
           * @struct nav2_smac_planner::HybridMotionTable
           * @brief A table of motion primitives and related functions
           */
          struct HybridMotionTable
          {
           /**
           * @brief A constructor for nav2_smac_planner::HybridMotionTable
           */
           HybridMotionTable(   ) {}
          
           /**
           * @brief Initializing using Dubin model
           * @param size_x_in Size of costmap in X
           * @param size_y_in Size of costmap in Y
           * @param angle_quantization_in Size of costmap in bin sizes
           * @param search_info Parameters for searching
           */
           void initDubin(  
           unsigned int & size_x_in,  
           unsigned int & size_y_in,  
           unsigned int & angle_quantization_in,  
           SearchInfo & search_info );
          
           /**
           * @brief Initializing using Reeds-Shepp model
           * @param size_x_in Size of costmap in X
           * @param size_y_in Size of costmap in Y
           * @param angle_quantization_in Size of costmap in bin sizes
           * @param search_info Parameters for searching
           */
           void initReedsShepp(  
           unsigned int & size_x_in,  
           unsigned int & size_y_in,  
           unsigned int & angle_quantization_in,  
           SearchInfo & search_info );
          
           /**
           * @brief Get projections of motion models
           * @param node Ptr to NodeHybrid
           * @return A set of motion poses
           */
           MotionPoses getProjections(  const NodeHybrid * node );
          
           /**
           * @brief Get the angular bin to use from a raw orientation
           * @param theta Angle in radians
           * @return bin index of closest angle to request
           */
           unsigned int getClosestAngularBin(  const double & theta );
          
           /**
           * @brief Get the raw orientation from an angular bin
           * @param bin_idx Index of the bin
           * @return Raw orientation in radians
           */
           float getAngleFromBin(  const unsigned int & bin_idx );
          
           MotionModel motion_model = MotionModel::UNKNOWN;
           MotionPoses projections;
           unsigned int size_x;
           unsigned int num_angle_quantization;
           float num_angle_quantization_float;
           float min_turning_radius;
           float bin_size;
           float change_penalty;
           float non_straight_penalty;
           float cost_penalty;
           float reverse_penalty;
           float travel_distance_reward;
           ompl::base::StateSpacePtr state_space;
           std::vector<std::vector<double>> delta_xs;
           std::vector<std::vector<double>> delta_ys;
           std::vector<TrigValues> trig_values;
          };
          
          /**
           * @class nav2_smac_planner::NodeHybrid
           * @brief NodeHybrid implementation for graph,   Hybrid-A*
           */
     135  class NodeHybrid
          {
          public:
           typedef NodeHybrid * NodePtr;
           typedef std::unique_ptr<std::vector<NodeHybrid>> Graph;
           typedef std::vector<NodePtr> NodeVector;
          
           /**
           * @class nav2_smac_planner::NodeHybrid::Coordinates
           * @brief NodeHybrid implementation of coordinate structure
           */
           struct Coordinates
           {
           /**
           * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
           */
           Coordinates(   ) {}
          
           /**
           * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
           * @param x_in X coordinate
           * @param y_in Y coordinate
           * @param theta_in Theta coordinate
           */
           Coordinates(  const float & x_in,   const float & y_in,   const float & theta_in )
           : x(  x_in ),   y(  y_in ),   theta(  theta_in )
           {}
          
     163   inline bool operator==(  const Coordinates & rhs )
           {
           return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
           }
          
     168   inline bool operator!=(  const Coordinates & rhs )
           {
           return !(  *this == rhs );
           }
          
           float x,   y,   theta;
           };
          
           typedef std::vector<Coordinates> CoordinateVector;
          
           /**
           * @brief A constructor for nav2_smac_planner::NodeHybrid
           * @param index The index of this node for self-reference
           */
           explicit NodeHybrid(  const unsigned int index );
          
           /**
           * @brief A destructor for nav2_smac_planner::NodeHybrid
           */
           ~NodeHybrid(   );
          
           /**
           * @brief operator== for comparisons
           * @param NodeHybrid right hand side node reference
           * @return If cell indicies are equal
           */
           bool operator==(  const NodeHybrid & rhs )
           {
           return this->_index == rhs._index;
           }
          
           /**
           * @brief setting continuous coordinate search poses (  in partial-cells )
           * @param Pose pose
           */
           inline void setPose(  const Coordinates & pose_in )
           {
           pose = pose_in;
           }
          
           /**
           * @brief Reset method for new search
           */
           void reset(   );
          
           /**
           * @brief Gets the accumulated cost at this node
           * @return accumulated cost
           */
           inline float & getAccumulatedCost(   )
           {
           return _accumulated_cost;
           }
          
           /**
           * @brief Sets the accumulated cost at this node
           * @param reference to accumulated cost
           */
           inline void setAccumulatedCost(  const float & cost_in )
           {
           _accumulated_cost = cost_in;
           }
          
           /**
           * @brief Sets the motion primitive index used to achieve node in search
           * @param reference to motion primitive idx
           */
           inline void setMotionPrimitiveIndex(  const unsigned int & idx )
           {
           _motion_primitive_index = idx;
           }
          
           /**
           * @brief Gets the motion primitive index used to achieve node in search
           * @return reference to motion primitive idx
           */
           inline unsigned int & getMotionPrimitiveIndex(   )
           {
           return _motion_primitive_index;
           }
          
           /**
           * @brief Gets the costmap cost at this node
           * @return costmap cost
           */
           inline float & getCost(   )
           {
           return _cell_cost;
           }
          
           /**
           * @brief Gets if cell has been visited in search
           * @param If cell was visited
           */
           inline bool & wasVisited(   )
           {
           return _was_visited;
           }
          
           /**
           * @brief Sets if cell has been visited in search
           */
           inline void visited(   )
           {
           _was_visited = true;
           }
          
           /**
           * @brief Gets cell index
           * @return Reference to cell index
           */
           inline unsigned int & getIndex(   )
           {
           return _index;
           }
          
           /**
           * @brief Check if this node is valid
           * @param traverse_unknown If we can explore unknown nodes on the graph
           * @return whether this node is valid and collision free
           */
           bool isNodeValid(  const bool & traverse_unknown,   GridCollisionChecker * collision_checker );
          
           /**
           * @brief Get traversal cost of parent node to child node
           * @param child Node pointer to child
           * @return traversal cost
           */
           float getTraversalCost(  const NodePtr & child );
          
           /**
           * @brief Get index at coordinates
           * @param x X coordinate of point
           * @param y Y coordinate of point
           * @param angle Theta coordinate of point
           * @param width Width of costmap
           * @param angle_quantization Number of theta bins
           * @return Index
           */
           static inline unsigned int getIndex(  
           const unsigned int & x,   const unsigned int & y,   const unsigned int & angle,  
           const unsigned int & width,   const unsigned int & angle_quantization )
           {
           return angle + x * angle_quantization + y * width * angle_quantization;
           }
          
           /**
           * @brief Get index at coordinates
           * @param x X coordinate of point
           * @param y Y coordinate of point
           * @param angle Theta coordinate of point
           * @return Index
           */
           static inline unsigned int getIndex(  
           const unsigned int & x,   const unsigned int & y,   const unsigned int & angle )
           {
           return getIndex(  
           x,   y,   angle,   motion_table.size_x,  
           motion_table.num_angle_quantization );
           }
          
           /**
           * @brief Get coordinates at index
           * @param index Index of point
           * @param width Width of costmap
           * @param angle_quantization Theta size of costmap
           * @return Coordinates
           */
           static inline Coordinates getCoords(  
           const unsigned int & index,  
           const unsigned int & width,   const unsigned int & angle_quantization )
           {
           return Coordinates(  
           (  index / angle_quantization ) % width,   // x
           index / (  angle_quantization * width ),   // y
           index % angle_quantization ); // theta
           }
          
           /**
           * @brief Get cost of heuristic of node
           * @param node Node index current
           * @param node Node index of new
           * @param costmap Costmap ptr to use
           * @return Heuristic cost between the nodes
           */
           static float getHeuristicCost(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coordinates,  
           const nav2_costmap_2d::Costmap2D * costmap );
          
           /**
           * @brief Initialize motion models
           * @param motion_model Motion model enum to use
           * @param size_x Size of X of graph
           * @param size_y Size of y of graph
           * @param angle_quantization Size of theta bins of graph
           * @param search_info Search info to use
           */
           static void initMotionModel(  
           const MotionModel & motion_model,  
           unsigned int & size_x,  
           unsigned int & size_y,  
           unsigned int & angle_quantization,  
           SearchInfo & search_info );
          
           /**
           * @brief Compute the SE2 distance heuristic
           * @param lookup_table_dim Size,   in costmap pixels,   of the
           * each lookup table dimension to populate
           * @param motion_model Motion model to use for state space
           * @param dim_3_size Number of quantization bins for caching
           * @param search_info Info containing minimum radius to use
           */
           static void precomputeDistanceHeuristic(  
           const float & lookup_table_dim,  
           const MotionModel & motion_model,  
           const unsigned int & dim_3_size,  
           const SearchInfo & search_info );
          
           /**
           * @brief Compute the Obstacle heuristic
           * @param node_coords Coordinates to get heuristic at
           * @param goal_coords Coordinates to compute heuristic to
           * @return heuristic Heuristic value
           */
           static float getObstacleHeuristic(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coords,  
           const double & cost_penalty );
          
           /**
           * @brief Compute the Distance heuristic
           * @param node_coords Coordinates to get heuristic at
           * @param goal_coords Coordinates to compute heuristic to
           * @param obstacle_heuristic Value of the obstacle heuristic to compute
           * additional motion heuristics if required
           * @return heuristic Heuristic value
           */
           static float getDistanceHeuristic(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coords,  
           const float & obstacle_heuristic );
          
           /**
           * @brief reset the obstacle heuristic state
           * @param costmap Costmap to use
           * @param goal_coords Coordinates to start heuristic expansion at
           */
           static void resetObstacleHeuristic(  
           nav2_costmap_2d::Costmap2D * costmap,  
           const unsigned int & start_x,   const unsigned int & start_y,  
           const unsigned int & goal_x,   const unsigned int & goal_y );
          
           /**
           * @brief Retrieve all valid neighbors of a node.
           * @param validity_checker Functor for state validity checking
           * @param collision_checker Collision checker to use
           * @param traverse_unknown If unknown costs are valid to traverse
           * @param neighbors Vector of neighbors to be filled
           */
           void getNeighbors(  
           std::function<bool(  const unsigned int &,   nav2_smac_planner::NodeHybrid * & )> & validity_checker,  
           GridCollisionChecker * collision_checker,  
           const bool & traverse_unknown,  
           NodeVector & neighbors );
          
           /**
           * @brief Set the starting pose for planning,   as a node index
           * @param path Reference to a vector of indicies of generated path
           * @return whether the path was able to be backtraced
           */
           bool backtracePath(  CoordinateVector & path );
          
           NodeHybrid * parent;
           Coordinates pose;
          
           // Constants required across all nodes but don't want to allocate more than once
           static double travel_distance_cost;
           static HybridMotionTable motion_table;
           // Wavefront lookup and queue for continuing to expand as needed
           static LookupTable obstacle_heuristic_lookup_table;
           static ObstacleHeuristicQueue obstacle_heuristic_queue;
          
           static nav2_costmap_2d::Costmap2D * sampled_costmap;
           static CostmapDownsampler downsampler;
           // Dubin / Reeds-Shepp lookup and size for dereferencing
           static LookupTable dist_heuristic_lookup_table;
           static float size_lookup;
          
          private:
           float _cell_cost;
           float _accumulated_cost;
           unsigned int _index;
           bool _was_visited;
           unsigned int _motion_primitive_index;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
          #define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
          
          #include <math.h>
          
          #include <vector>
          #include <cmath>
          #include <iostream>
          #include <functional>
          #include <queue>
          #include <memory>
          #include <utility>
          #include <limits>
          #include <string>
          
          #include "nlohmann/json.hpp"
          #include "ompl/base/StateSpace.h"
          #include "angles/angles.h"
          
          #include "nav2_smac_planner/constants.hpp"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/utils.hpp"
          
          namespace nav2_smac_planner
          {
          
          // forward declare
      44  class NodeLattice;
      45  class NodeHybrid;
          
          /**
           * @struct nav2_smac_planner::LatticeMotionTable
           * @brief A table of motion primitives and related functions
           */
          struct LatticeMotionTable
          {
           /**
           * @brief A constructor for nav2_smac_planner::LatticeMotionTable
           */
           LatticeMotionTable(   ) {}
          
           /**
           * @brief Initializing state lattice planner's motion model
           * @param size_x_in Size of costmap in X
           * @param size_y_in Size of costmap in Y
           * @param angle_quantization_in Size of costmap in bin sizes
           * @param search_info Parameters for searching
           */
           void initMotionModel(  
           unsigned int & size_x_in,  
           SearchInfo & search_info );
          
           /**
           * @brief Get projections of motion models
           * @param node Ptr to NodeLattice
           * @return A set of motion poses
           */
           MotionPrimitivePtrs getMotionPrimitives(  const NodeLattice * node );
          
           /**
           * @brief Get file metadata needed
           * @param lattice_filepath Filepath to the lattice file
           * @return A set of metadata containing the number of angular bins
           * and the global coordinates minimum turning radius of the primitives
           * for use in analytic expansion and heuristic calculation.
           */
           static LatticeMetadata getLatticeMetadata(  const std::string & lattice_filepath );
          
           /**
           * @brief Get the angular bin to use from a raw orientation
           * @param theta Angle in radians
           * @return bin index of closest angle to request
           */
           unsigned int getClosestAngularBin(  const double & theta );
          
           /**
           * @brief Get the raw orientation from an angular bin
           * @param bin_idx Index of the bin
           * @return Raw orientation in radians
           */
           float & getAngleFromBin(  const unsigned int & bin_idx );
          
           unsigned int size_x;
           unsigned int num_angle_quantization;
           float change_penalty;
           float non_straight_penalty;
           float cost_penalty;
           float reverse_penalty;
           float travel_distance_reward;
           float rotation_penalty;
           bool allow_reverse_expansion;
           std::vector<std::vector<MotionPrimitive>> motion_primitives;
           ompl::base::StateSpacePtr state_space;
           std::vector<TrigValues> trig_values;
           std::string current_lattice_filepath;
           LatticeMetadata lattice_metadata;
          };
          
          /**
           * @class nav2_smac_planner::NodeLattice
           * @brief NodeLattice implementation for graph,   Hybrid-A*
           */
     119  class NodeLattice
          {
          public:
           typedef NodeLattice * NodePtr;
           typedef std::unique_ptr<std::vector<NodeLattice>> Graph;
           typedef std::vector<NodePtr> NodeVector;
           typedef NodeHybrid::Coordinates Coordinates;
           typedef NodeHybrid::CoordinateVector CoordinateVector;
          
           /**
           * @brief A constructor for nav2_smac_planner::NodeLattice
           * @param index The index of this node for self-reference
           */
           explicit NodeLattice(  const unsigned int index );
          
           /**
           * @brief A destructor for nav2_smac_planner::NodeLattice
           */
           ~NodeLattice(   );
          
           /**
           * @brief operator== for comparisons
           * @param NodeLattice right hand side node reference
           * @return If cell indicies are equal
           */
           bool operator==(  const NodeLattice & rhs )
           {
           return this->_index == rhs._index;
           }
          
           /**
           * @brief setting continuous coordinate search poses (  in partial-cells )
           * @param Pose pose
           */
           inline void setPose(  const Coordinates & pose_in )
           {
           pose = pose_in;
           }
          
           /**
           * @brief Reset method for new search
           */
     161   void reset(   );
          
           /**
           * @brief Sets the motion primitive used to achieve node in search
           * @param pointer to motion primitive
           */
     167   inline void setMotionPrimitive(  MotionPrimitive * prim )
           {
           _motion_primitive = prim;
           }
          
           /**
           * @brief Gets the motion primitive used to achieve node in search
           * @return pointer to motion primitive
           */
     176   inline MotionPrimitive * & getMotionPrimitive(   )
           {
           return _motion_primitive;
           }
          
           /**
           * @brief Gets the accumulated cost at this node
           * @return accumulated cost
           */
     185   inline float & getAccumulatedCost(   )
           {
           return _accumulated_cost;
           }
          
           /**
           * @brief Sets the accumulated cost at this node
           * @param reference to accumulated cost
           */
     194   inline void setAccumulatedCost(  const float & cost_in )
           {
           _accumulated_cost = cost_in;
           }
          
           /**
           * @brief Gets the costmap cost at this node
           * @return costmap cost
           */
     203   inline float & getCost(   )
           {
           return _cell_cost;
           }
          
           /**
           * @brief Gets if cell has been visited in search
           * @param If cell was visited
           */
     212   inline bool & wasVisited(   )
           {
           return _was_visited;
           }
          
           /**
           * @brief Sets if cell has been visited in search
           */
     220   inline void visited(   )
           {
           _was_visited = true;
           }
          
           /**
           * @brief Gets cell index
           * @return Reference to cell index
           */
     229   inline unsigned int & getIndex(   )
           {
           return _index;
           }
          
           /**
           * @brief Sets that this primitive is moving in reverse
           */
     237   inline void backwards(  bool back = true )
           {
           _backwards = back;
           }
          
           /**
           * @brief Gets if this primitive is moving in reverse
           * @return backwards If moving in reverse
           */
     246   inline bool isBackward(   )
           {
           return _backwards;
           }
          
           /**
           * @brief Check if this node is valid
           * @param traverse_unknown If we can explore unknown nodes on the graph
           * @param collision_checker Collision checker object to aid in validity checking
           * @param primitive Optional argument if needing to check over a primitive
           * not only a terminal pose
           * @param is_backwards Optional argument if needed to check if prim expansion is
           * in reverse
           * @return whether this node is valid and collision free
           */
     261   bool isNodeValid(  
           const bool & traverse_unknown,  
           GridCollisionChecker * collision_checker,  
           MotionPrimitive * primitive = nullptr,  
           bool is_backwards = false );
          
           /**
           * @brief Get traversal cost of parent node to child node
           * @param child Node pointer to child
           * @return traversal cost
           */
     272   float getTraversalCost(  const NodePtr & child );
          
           /**
           * @brief Get index at coordinates
           * @param x X coordinate of point
           * @param y Y coordinate of point
           * @param angle Theta coordinate of point
           * @return Index
           */
     281   static inline unsigned int getIndex(  
           const unsigned int & x,   const unsigned int & y,   const unsigned int & angle )
           {
           // Hybrid-A* and State Lattice share a coordinate system
           return NodeHybrid::getIndex(  
           x,   y,   angle,   motion_table.size_x,  
           motion_table.num_angle_quantization );
           }
          
           /**
           * @brief Get coordinates at index
           * @param index Index of point
           * @param width Width of costmap
           * @param angle_quantization Theta size of costmap
           * @return Coordinates
           */
     297   static inline Coordinates getCoords(  
           const unsigned int & index,  
           const unsigned int & width,   const unsigned int & angle_quantization )
           {
           // Hybrid-A* and State Lattice share a coordinate system
           return NodeHybrid::Coordinates(  
           (  index / angle_quantization ) % width,   // x
           index / (  angle_quantization * width ),   // y
           index % angle_quantization ); // theta
           }
          
           /**
           * @brief Get cost of heuristic of node
           * @param node Node index current
           * @param node Node index of new
           * @param costmap Costmap ptr to use
           * @return Heuristic cost between the nodes
           */
     315   static float getHeuristicCost(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coordinates,  
           const nav2_costmap_2d::Costmap2D * costmap );
          
           /**
           * @brief Initialize motion models
           * @param motion_model Motion model enum to use
           * @param size_x Size of X of graph
           * @param size_y Size of y of graph
           * @param angle_quantization Size of theta bins of graph
           * @param search_info Search info to use
           */
     328   static void initMotionModel(  
           const MotionModel & motion_model,  
           unsigned int & size_x,  
           unsigned int & size_y,  
           unsigned int & angle_quantization,  
           SearchInfo & search_info );
          
           /**
           * @brief Compute the SE2 distance heuristic
           * @param lookup_table_dim Size,   in costmap pixels,   of the
           * each lookup table dimension to populate
           * @param motion_model Motion model to use for state space
           * @param dim_3_size Number of quantization bins for caching
           * @param search_info Info containing minimum radius to use
           */
     343   static void precomputeDistanceHeuristic(  
           const float & lookup_table_dim,  
           const MotionModel & motion_model,  
           const unsigned int & dim_3_size,  
           const SearchInfo & search_info );
          
           /**
           * @brief Compute the wavefront heuristic
           * @param costmap Costmap to use
           * @param goal_coords Coordinates to start heuristic expansion at
           */
     354   static void resetObstacleHeuristic(  
           nav2_costmap_2d::Costmap2D * costmap,  
           const unsigned int & start_x,   const unsigned int & start_y,  
           const unsigned int & goal_x,   const unsigned int & goal_y )
           {
           // State Lattice and Hybrid-A* share this heuristics
           NodeHybrid::resetObstacleHeuristic(  costmap,   start_x,   start_y,   goal_x,   goal_y );
           }
          
           /**
           * @brief Compute the Obstacle heuristic
           * @param node_coords Coordinates to get heuristic at
           * @param goal_coords Coordinates to compute heuristic to
           * @return heuristic Heuristic value
           */
     369   static float getObstacleHeuristic(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coords,  
           const double & cost_penalty )
           {
           return NodeHybrid::getObstacleHeuristic(  node_coords,   goal_coords,   cost_penalty );
           }
          
           /**
           * @brief Compute the Distance heuristic
           * @param node_coords Coordinates to get heuristic at
           * @param goal_coords Coordinates to compute heuristic to
           * @param obstacle_heuristic Value of the obstacle heuristic to compute
           * additional motion heuristics if required
           * @return heuristic Heuristic value
           */
     385   static float getDistanceHeuristic(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coords,  
           const float & obstacle_heuristic );
          
           /**
           * @brief Retrieve all valid neighbors of a node.
           * @param validity_checker Functor for state validity checking
           * @param collision_checker Collision checker to use
           * @param traverse_unknown If unknown costs are valid to traverse
           * @param neighbors Vector of neighbors to be filled
           */
     397   void getNeighbors(  
           std::function<bool(  const unsigned int &,  
           nav2_smac_planner::NodeLattice * & )> & validity_checker,  
           GridCollisionChecker * collision_checker,  
           const bool & traverse_unknown,  
           NodeVector & neighbors );
          
           /**
           * @brief Set the starting pose for planning,   as a node index
           * @param path Reference to a vector of indicies of generated path
           * @return whether the path was able to be backtraced
           */
     409   bool backtracePath(  CoordinateVector & path );
          
           NodeLattice * parent;
           Coordinates pose;
           static LatticeMotionTable motion_table;
           // Dubin / Reeds-Shepp lookup and size for dereferencing
           static LookupTable dist_heuristic_lookup_table;
           static float size_lookup;
          
          private:
           float _cell_cost;
           float _accumulated_cost;
           unsigned int _index;
           bool _was_visited;
           MotionPrimitive * _motion_primitive;
           bool _backwards;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
          #define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
          
          #include <memory>
          #include <vector>
          #include <string>
          #include <mutex>
          
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/smoother.hpp"
          #include "nav2_smac_planner/utils.hpp"
          #include "nav2_smac_planner/costmap_downsampler.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "tf2/utils.h"
          #include "rcl_interfaces/msg/set_parameters_result.hpp"
          
          namespace nav2_smac_planner
          {
          
      41  class SmacPlanner2D : public nav2_core::GlobalPlanner
          {
          public:
           /**
           * @brief constructor
           */
      47   SmacPlanner2D(   );
          
           /**
           * @brief destructor
           */
      52   ~SmacPlanner2D(   );
          
           /**
           * @brief Configuring plugin
           * @param parent Lifecycle node pointer
           * @param name Name of plugin map
           * @param tf Shared ptr of TF2 buffer
           * @param costmap_ros Costmap2DROS object
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup lifecycle node
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate lifecycle node
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate lifecycle node
           */
           void deactivate(   ) override;
          
           /**
           * @brief Creating a plan from start and goal poses
           * @param start Start pose
           * @param goal Goal pose
           * @return nav2_msgs::Path of the generated path
           */
           nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) override;
          
          protected:
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           std::unique_ptr<AStarAlgorithm<Node2D>> _a_star;
           GridCollisionChecker _collision_checker;
           std::unique_ptr<Smoother> _smoother;
           nav2_costmap_2d::Costmap2D * _costmap;
           std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
           rclcpp::Clock::SharedPtr _clock;
           rclcpp::Logger _logger{rclcpp::get_logger(  "SmacPlanner2D" )};
           std::string _global_frame,   _name;
           float _tolerance;
           int _downsampling_factor;
           bool _downsample_costmap;
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
           double _max_planning_time;
           bool _allow_unknown;
           int _max_iterations;
           int _max_on_approach_iterations;
           bool _use_final_approach_orientation;
           SearchInfo _search_info;
           std::string _motion_model_for_search;
           MotionModel _motion_model;
           std::mutex _mutex;
           rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
          #define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
          
          #include <memory>
          #include <vector>
          #include <string>
          
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/smoother.hpp"
          #include "nav2_smac_planner/utils.hpp"
          #include "nav2_smac_planner/costmap_downsampler.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "tf2/utils.h"
          
          namespace nav2_smac_planner
          {
          
      39  class SmacPlannerHybrid : public nav2_core::GlobalPlanner
          {
          public:
           /**
           * @brief constructor
           */
      45   SmacPlannerHybrid(   );
          
           /**
           * @brief destructor
           */
      50   ~SmacPlannerHybrid(   );
          
           /**
           * @brief Configuring plugin
           * @param parent Lifecycle node pointer
           * @param name Name of plugin map
           * @param tf Shared ptr of TF2 buffer
           * @param costmap_ros Costmap2DROS object
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup lifecycle node
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate lifecycle node
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate lifecycle node
           */
           void deactivate(   ) override;
          
           /**
           * @brief Creating a plan from start and goal poses
           * @param start Start pose
           * @param goal Goal pose
           * @return nav2_msgs::Path of the generated path
           */
           nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) override;
          
          protected:
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           std::unique_ptr<AStarAlgorithm<NodeHybrid>> _a_star;
           GridCollisionChecker _collision_checker;
           std::unique_ptr<Smoother> _smoother;
           rclcpp::Clock::SharedPtr _clock;
           rclcpp::Logger _logger{rclcpp::get_logger(  "SmacPlannerHybrid" )};
           nav2_costmap_2d::Costmap2D * _costmap;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
           std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
           std::string _global_frame,   _name;
           float _lookup_table_dim;
           float _tolerance;
           bool _downsample_costmap;
           int _downsampling_factor;
           double _angle_bin_size;
           unsigned int _angle_quantizations;
           bool _allow_unknown;
           int _max_iterations;
           SearchInfo _search_info;
           double _max_planning_time;
           double _lookup_table_size;
           double _minimum_turning_radius_global_coords;
           std::string _motion_model_for_search;
           MotionModel _motion_model;
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
           std::mutex _mutex;
           rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

          // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
          #define NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
          
          #include <memory>
          #include <vector>
          #include <string>
          
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/smoother.hpp"
          #include "nav2_smac_planner/utils.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "tf2/utils.h"
          
          namespace nav2_smac_planner
          {
          
      38  class SmacPlannerLattice : public nav2_core::GlobalPlanner
          {
          public:
           /**
           * @brief constructor
           */
      44   SmacPlannerLattice(   );
          
           /**
           * @brief destructor
           */
      49   ~SmacPlannerLattice(   );
          
           /**
           * @brief Configuring plugin
           * @param parent Lifecycle node pointer
           * @param name Name of plugin map
           * @param tf Shared ptr of TF2 buffer
           * @param costmap_ros Costmap2DROS object
           */
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           /**
           * @brief Cleanup lifecycle node
           */
           void cleanup(   ) override;
          
           /**
           * @brief Activate lifecycle node
           */
           void activate(   ) override;
          
           /**
           * @brief Deactivate lifecycle node
           */
           void deactivate(   ) override;
          
           /**
           * @brief Creating a plan from start and goal poses
           * @param start Start pose
           * @param goal Goal pose
           * @return nav2_msgs::Path of the generated path
           */
           nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) override;
          
          protected:
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           std::unique_ptr<AStarAlgorithm<NodeLattice>> _a_star;
           GridCollisionChecker _collision_checker;
           std::unique_ptr<Smoother> _smoother;
           rclcpp::Clock::SharedPtr _clock;
           rclcpp::Logger _logger{rclcpp::get_logger(  "SmacPlannerLattice" )};
           nav2_costmap_2d::Costmap2D * _costmap;
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
           MotionModel _motion_model;
           LatticeMetadata _metadata;
           std::string _global_frame,   _name;
           SearchInfo _search_info;
           bool _allow_unknown;
           int _max_iterations;
           float _tolerance;
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
           double _max_planning_time;
           double _lookup_table_size;
           std::mutex _mutex;
           rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/types.hpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__TYPES_HPP_
          #define NAV2_SMAC_PLANNER__TYPES_HPP_
          
          #include <vector>
          #include <utility>
          #include <string>
          #include <memory>
          
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_smac_planner
          {
          
          typedef std::pair<float,   unsigned int> NodeHeuristicPair;
          
          /**
           * @struct nav2_smac_planner::SearchInfo
           * @brief Search properties and penalties
           */
          struct SearchInfo
          {
           float minimum_turning_radius;
           float non_straight_penalty;
           float change_penalty;
           float reverse_penalty;
           float cost_penalty;
           float retrospective_penalty;
           float rotation_penalty;
           float analytic_expansion_ratio;
           float analytic_expansion_max_length;
           std::string lattice_filepath;
           bool cache_obstacle_heuristic;
           bool allow_reverse_expansion;
          };
          
          /**
           * @struct nav2_smac_planner::SmootherParams
           * @brief Parameters for the smoother
           */
          struct SmootherParams
          {
           /**
           * @brief A constructor for nav2_smac_planner::SmootherParams
           */
           SmootherParams(   )
           : holonomic_(  false )
           {
           }
          
           /**
           * @brief Get params from ROS parameter
           * @param node Ptr to node
           * @param name Name of plugin
           */
           void get(  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,   const std::string & name )
           {
           std::string local_name = name + std::string(  ".smoother." );
          
           // Smoother params
           nav2_util::declare_parameter_if_not_declared(  
           node,   local_name + "tolerance",   rclcpp::ParameterValue(  1e-10 ) );
           node->get_parameter(  local_name + "tolerance",   tolerance_ );
           nav2_util::declare_parameter_if_not_declared(  
           node,   local_name + "max_iterations",   rclcpp::ParameterValue(  1000 ) );
           node->get_parameter(  local_name + "max_iterations",   max_its_ );
           nav2_util::declare_parameter_if_not_declared(  
           node,   local_name + "w_data",   rclcpp::ParameterValue(  0.2 ) );
           node->get_parameter(  local_name + "w_data",   w_data_ );
           nav2_util::declare_parameter_if_not_declared(  
           node,   local_name + "w_smooth",   rclcpp::ParameterValue(  0.3 ) );
           node->get_parameter(  local_name + "w_smooth",   w_smooth_ );
           nav2_util::declare_parameter_if_not_declared(  
           node,   local_name + "do_refinement",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  local_name + "do_refinement",   do_refinement_ );
           }
          
           double tolerance_;
           int max_its_;
           double w_data_;
           double w_smooth_;
           bool holonomic_;
           bool do_refinement_;
          };
          
          /**
           * @struct nav2_smac_planner::MotionPose
           * @brief A struct for poses in motion primitives
           */
          struct MotionPose
          {
           /**
           * @brief A constructor for nav2_smac_planner::MotionPose
           */
           MotionPose(   ) {}
          
           /**
           * @brief A constructor for nav2_smac_planner::MotionPose
           * @param x X pose
           * @param y Y pose
           * @param theta Angle of pose
           */
           MotionPose(  const float & x,   const float & y,   const float & theta )
           : _x(  x ),   _y(  y ),   _theta(  theta )
           {}
          
           MotionPose operator-(  const MotionPose & p2 )
           {
           return MotionPose(  this->_x - p2._x,   this->_y - p2._y,   this->_theta - p2._theta );
           }
          
           float _x;
           float _y;
           float _theta;
          };
          
          typedef std::vector<MotionPose> MotionPoses;
          
          /**
           * @struct nav2_smac_planner::LatticeMetadata
           * @brief A struct of all lattice metadata
           */
          struct LatticeMetadata
          {
           float min_turning_radius;
           float grid_resolution;
           unsigned int number_of_headings;
           std::vector<float> heading_angles;
           unsigned int number_of_trajectories;
           std::string motion_model;
          };
          
          /**
           * @struct nav2_smac_planner::MotionPrimitive
           * @brief A struct of all motion primitive data
           */
          struct MotionPrimitive
          {
           unsigned int trajectory_id;
           float start_angle;
           float end_angle;
           float turning_radius;
           float trajectory_length;
           float arc_length;
           float straight_length;
           bool left_turn;
           MotionPoses poses;
          };
          
          typedef std::vector<MotionPrimitive> MotionPrimitives;
          typedef std::vector<MotionPrimitive *> MotionPrimitivePtrs;
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__TYPES_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/utils.hpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMAC_PLANNER__UTILS_HPP_
          #define NAV2_SMAC_PLANNER__UTILS_HPP_
          
          #include <vector>
          #include <memory>
          
          #include "Eigen/Core"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "geometry_msgs/msg/pose.hpp"
          #include "tf2/utils.h"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/inflation_layer.hpp"
          
          namespace nav2_smac_planner
          {
          
          /**
          * @brief Create an Eigen Vector2D of world poses from continuous map coords
          * @param mx float of map X coordinate
          * @param my float of map Y coordinate
          * @param costmap Costmap pointer
          * @return Eigen::Vector2d eigen vector of the generated path
          */
      38  inline geometry_msgs::msg::Pose getWorldCoords(  
           const float & mx,   const float & my,   const nav2_costmap_2d::Costmap2D * costmap )
          {
           geometry_msgs::msg::Pose msg;
           msg.position.x =
           static_cast<float>(  costmap->getOriginX(   ) ) + (  mx + 0.5 ) * costmap->getResolution(   );
           msg.position.y =
           static_cast<float>(  costmap->getOriginY(   ) ) + (  my + 0.5 ) * costmap->getResolution(   );
           return msg;
          }
          
          /**
          * @brief Create quaternion from radians
          * @param theta continuous bin coordinates angle
          * @return quaternion orientation in map frame
          */
      54  inline geometry_msgs::msg::Quaternion getWorldOrientation(  
           const float & theta )
          {
           // theta is in radians already
           tf2::Quaternion q;
           q.setEuler(  0.0,   0.0,   theta );
           return tf2::toMsg(  q );
          }
          
          /**
          * @brief Find the min cost of the inflation decay function for which the robot MAY be
          * in collision in any orientation
          * @param costmap Costmap2DROS to get minimum inscribed cost (  e.g. 128 in inflation layer documentation )
          * @return double circumscribed cost,   any higher than this and need to do full footprint collision checking
          * since some element of the robot could be in collision
          */
      70  inline double findCircumscribedCost(  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap )
          {
           double result = -1.0;
           bool inflation_layer_found = false;
           std::vector<std::shared_ptr<nav2_costmap_2d::Layer>>::iterator layer;
          
           // check if the costmap has an inflation layer
           for (  layer = costmap->getLayeredCostmap(   )->getPlugins(   )->begin(   );
           layer != costmap->getLayeredCostmap(   )->getPlugins(   )->end(   );
           ++layer )
           {
           std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer =
           std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(  *layer );
           if (  !inflation_layer ) {
           continue;
           }
          
           inflation_layer_found = true;
           double circum_radius = costmap->getLayeredCostmap(   )->getCircumscribedRadius(   );
           double resolution = costmap->getCostmap(   )->getResolution(   );
           result = static_cast<double>(  inflation_layer->computeCost(  circum_radius / resolution ) );
           }
          
           if (  !inflation_layer_found ) {
           RCLCPP_WARN(  
           rclcpp::get_logger(  "computeCircumscribedCost" ),  
           "No inflation layer found in costmap configuration. "
           "If this is an SE2-collision checking plugin,   it cannot use costmap potential "
           "field to speed up collision checking by only checking the full footprint "
           "when robot is within possibly-inscribed radius of an obstacle. This may "
           "significantly slow down planning times!" );
           }
          
           return result;
          }
          
          /**
           * @brief convert json to lattice metadata
           * @param[in] json json object
           * @param[out] lattice meta data
           */
     111  inline void fromJsonToMetaData(  const nlohmann::json & json,   LatticeMetadata & lattice_metadata )
          {
           json.at(  "turning_radius" ).get_to(  lattice_metadata.min_turning_radius );
           json.at(  "grid_resolution" ).get_to(  lattice_metadata.grid_resolution );
           json.at(  "num_of_headings" ).get_to(  lattice_metadata.number_of_headings );
           json.at(  "heading_angles" ).get_to(  lattice_metadata.heading_angles );
           json.at(  "number_of_trajectories" ).get_to(  lattice_metadata.number_of_trajectories );
           json.at(  "motion_model" ).get_to(  lattice_metadata.motion_model );
          }
          
          /**
           * @brief convert json to pose
           * @param[in] json json object
           * @param[out] pose
           */
     126  inline void fromJsonToPose(  const nlohmann::json & json,   MotionPose & pose )
          {
           pose._x = json[0];
           pose._y = json[1];
           pose._theta = json[2];
          }
          
          /**
           * @brief convert json to motion primitive
           * @param[in] json json object
           * @param[out] motion primitive
           */
     138  inline void fromJsonToMotionPrimitive(  
           const nlohmann::json & json,   MotionPrimitive & motion_primitive )
          {
           json.at(  "trajectory_id" ).get_to(  motion_primitive.trajectory_id );
           json.at(  "start_angle_index" ).get_to(  motion_primitive.start_angle );
           json.at(  "end_angle_index" ).get_to(  motion_primitive.end_angle );
           json.at(  "trajectory_radius" ).get_to(  motion_primitive.turning_radius );
           json.at(  "trajectory_length" ).get_to(  motion_primitive.trajectory_length );
           json.at(  "arc_length" ).get_to(  motion_primitive.arc_length );
           json.at(  "straight_length" ).get_to(  motion_primitive.straight_length );
           json.at(  "left_turn" ).get_to(  motion_primitive.left_turn );
          
           for (  unsigned int i = 0; i < json["poses"].size(   ); i++ ) {
           MotionPose pose;
           fromJsonToPose(  json["poses"][i],   pose );
           motion_primitive.poses.push_back(  pose );
           }
          }
          
          } // namespace nav2_smac_planner
          
          #endif // NAV2_SMAC_PLANNER__UTILS_HPP_

navigation2/nav2_smac_planner/src/a_star.cpp

          // Copyright (  c ) 2020,   Samsung Research America
          // Copyright (  c ) 2020,   Applied Electric Vehicles Pty Ltd
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <omp.h>
          #include <cmath>
          #include <stdexcept>
          #include <memory>
          #include <algorithm>
          #include <limits>
          #include <type_traits>
          #include <chrono>
          #include <thread>
          #include <utility>
          #include <vector>
          
          #include "nav2_smac_planner/a_star.hpp"
          using namespace std::chrono; // NOLINT
          
          namespace nav2_smac_planner
          {
          
          template<typename NodeT>
      35  AStarAlgorithm<NodeT>::AStarAlgorithm(  
      36   const MotionModel & motion_model,  
           const SearchInfo & search_info )
          : _traverse_unknown(  true ),  
           _max_iterations(  0 ),  
           _max_planning_time(  0 ),  
           _x_size(  0 ),  
           _y_size(  0 ),  
           _search_info(  search_info ),  
           _goal_coordinates(  Coordinates(   ) ),  
           _start(  nullptr ),  
           _goal(  nullptr ),  
           _motion_model(  motion_model )
          {
           _graph.reserve(  100000 );
          }
          
          template<typename NodeT>
      53  AStarAlgorithm<NodeT>::~AStarAlgorithm(   )
          {
          }
          
          template<typename NodeT>
      58  void AStarAlgorithm<NodeT>::initialize(  
      59   const bool & allow_unknown,  
           int & max_iterations,  
           const int & max_on_approach_iterations,  
           const double & max_planning_time,  
           const float & lookup_table_size,  
           const unsigned int & dim_3_size )
          {
           _traverse_unknown = allow_unknown;
           _max_iterations = max_iterations;
           _max_on_approach_iterations = max_on_approach_iterations;
           _max_planning_time = max_planning_time;
           NodeT::precomputeDistanceHeuristic(  lookup_table_size,   _motion_model,   dim_3_size,   _search_info );
           _dim3_size = dim_3_size;
           _expander = std::make_unique<AnalyticExpansion<NodeT>>(  
           _motion_model,   _search_info,   _traverse_unknown,   _dim3_size );
          }
          
          template<>
      77  void AStarAlgorithm<Node2D>::initialize(  
      78   const bool & allow_unknown,  
           int & max_iterations,  
           const int & max_on_approach_iterations,  
           const double & max_planning_time,  
           const float & /*lookup_table_size*/,  
           const unsigned int & dim_3_size )
          {
           _traverse_unknown = allow_unknown;
           _max_iterations = max_iterations;
           _max_on_approach_iterations = max_on_approach_iterations;
           _max_planning_time = max_planning_time;
          
           if (  dim_3_size != 1 ) {
           throw std::runtime_error(  "Node type Node2D cannot be given non-1 dim 3 quantization." );
           }
           _dim3_size = dim_3_size;
           _expander = std::make_unique<AnalyticExpansion<Node2D>>(  
           _motion_model,   _search_info,   _traverse_unknown,   _dim3_size );
          }
          
          template<typename NodeT>
      99  void AStarAlgorithm<NodeT>::setCollisionChecker(  GridCollisionChecker * collision_checker )
          {
           _collision_checker = collision_checker;
           _costmap = collision_checker->getCostmap(   );
           unsigned int x_size = _costmap->getSizeInCellsX(   );
           unsigned int y_size = _costmap->getSizeInCellsY(   );
          
           clearGraph(   );
          
           if (  getSizeX(   ) != x_size || getSizeY(   ) != y_size ) {
           _x_size = x_size;
           _y_size = y_size;
           NodeT::initMotionModel(  _motion_model,   _x_size,   _y_size,   _dim3_size,   _search_info );
           }
           _expander->setCollisionChecker(  collision_checker );
          }
          
          template<typename NodeT>
          typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(  
           const unsigned int & index )
          {
           // Emplace will only create a new object if it doesn't already exist.
           // If an element exists,   it will return the existing object,   not create a new one.
     122   return &(  _graph.emplace(  index,   NodeT(  index ) ).first->second );
          }
          
          template<>
          void AStarAlgorithm<Node2D>::setStart(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 )
          {
           if (  dim_3 != 0 ) {
           throw std::runtime_error(  "Node type Node2D cannot be given non-zero starting dim 3." );
           }
           _start = addToGraph(  Node2D::getIndex(  mx,   my,   getSizeX(   ) ) );
          }
          
          template<typename NodeT>
          void AStarAlgorithm<NodeT>::setStart(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 )
          {
           _start = addToGraph(  NodeT::getIndex(  mx,   my,   dim_3 ) );
           _start->setPose(  
           Coordinates(  
           static_cast<float>(  mx ),  
           static_cast<float>(  my ),  
           static_cast<float>(  dim_3 ) ) );
          }
          
          template<>
          void AStarAlgorithm<Node2D>::setGoal(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 )
          {
           if (  dim_3 != 0 ) {
           throw std::runtime_error(  "Node type Node2D cannot be given non-zero goal dim 3." );
           }
          
           _goal = addToGraph(  Node2D::getIndex(  mx,   my,   getSizeX(   ) ) );
           _goal_coordinates = Node2D::Coordinates(  mx,   my );
          }
          
          template<typename NodeT>
          void AStarAlgorithm<NodeT>::setGoal(  
           const unsigned int & mx,  
           const unsigned int & my,  
           const unsigned int & dim_3 )
          {
           _goal = addToGraph(  NodeT::getIndex(  mx,   my,   dim_3 ) );
          
           typename NodeT::Coordinates goal_coords(  
           static_cast<float>(  mx ),  
           static_cast<float>(  my ),  
           static_cast<float>(  dim_3 ) );
          
           if (  !_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates ) {
           if (  !_start ) {
           throw std::runtime_error(  "Start must be set before goal." );
           }
          
           NodeT::resetObstacleHeuristic(  _costmap,   _start->pose.x,   _start->pose.y,   mx,   my );
           }
          
           _goal_coordinates = goal_coords;
           _goal->setPose(  _goal_coordinates );
          }
          
          template<typename NodeT>
          bool AStarAlgorithm<NodeT>::areInputsValid(   )
          {
           // Check if graph was filled in
           if (  _graph.empty(   ) ) {
           throw std::runtime_error(  "Failed to compute path,   no costmap given." );
           }
          
           // Check if points were filled in
           if (  !_start || !_goal ) {
           throw std::runtime_error(  "Failed to compute path,   no valid start or goal given." );
           }
          
           // Check if ending point is valid
           if (  getToleranceHeuristic(   ) < 0.001 &&
           !_goal->isNodeValid(  _traverse_unknown,   _collision_checker ) )
           {
           throw std::runtime_error(  "Failed to compute path,   goal is occupied with no tolerance." );
           }
          
           // Check if starting point is valid
           if (  !_start->isNodeValid(  _traverse_unknown,   _collision_checker ) ) {
           throw std::runtime_error(  "Starting point in lethal space! Cannot create feasible plan." );
           }
          
           return true;
          }
          
          template<typename NodeT>
          bool AStarAlgorithm<NodeT>::createPath(  
           CoordinateVector & path,   int & iterations,  
           const float & tolerance )
          {
           steady_clock::time_point start_time = steady_clock::now(   );
           _tolerance = tolerance;
           _best_heuristic_node = {std::numeric_limits<float>::max(   ),   0};
           clearQueue(   );
          
           if (  !areInputsValid(   ) ) {
           return false;
           }
          
           // 0 ) Add starting point to the open set
           addNode(  0.0,   getStart(   ) );
           getStart(   )->setAccumulatedCost(  0.0 );
          
           // Optimization: preallocate all variables
           NodePtr current_node = nullptr;
           NodePtr neighbor = nullptr;
           NodePtr expansion_result = nullptr;
           float g_cost = 0.0;
           NodeVector neighbors;
           int approach_iterations = 0;
           NeighborIterator neighbor_iterator;
           int analytic_iterations = 0;
           int closest_distance = std::numeric_limits<int>::max(   );
          
           // Given an index,   return a node ptr reference if its collision-free and valid
           const unsigned int max_index = getSizeX(   ) * getSizeY(   ) * getSizeDim3(   );
           NodeGetter neighborGetter =
           [&,   this](  const unsigned int & index,   NodePtr & neighbor_rtn ) -> bool
           {
           if (  index >= max_index ) {
           return false;
           }
          
           neighbor_rtn = addToGraph(  index );
           return true;
           };
          
           while (  iterations < getMaxIterations(   ) && !_queue.empty(   ) ) {
           // Check for planning timeout only on every Nth iteration
           if (  iterations % _timing_interval == 0 ) {
           std::chrono::duration<double> planning_duration =
           std::chrono::duration_cast<std::chrono::duration<double>>(  steady_clock::now(   ) - start_time );
           if (  static_cast<double>(  planning_duration.count(   ) ) >= _max_planning_time ) {
           return false;
           }
           }
          
           // 1 ) Pick Nbest from O s.t. min(  f(  Nbest ) ),   remove from queue
           current_node = getNextNode(   );
          
           // We allow for nodes to be queued multiple times in case
           // shorter paths result in it,   but we can visit only once
           if (  current_node->wasVisited(   ) ) {
           continue;
           }
          
           iterations++;
          
           // 2 ) Mark Nbest as visited
           current_node->visited(   );
          
           // 2.1 ) Use an analytic expansion (  if available ) to generate a path
           expansion_result = nullptr;
           expansion_result = _expander->tryAnalyticExpansion(  
           current_node,   getGoal(   ),   neighborGetter,   analytic_iterations,   closest_distance );
           if (  expansion_result != nullptr ) {
           current_node = expansion_result;
           }
          
           // 3 ) Check if we're at the goal,   backtrace if required
           if (  isGoal(  current_node ) ) {
           return current_node->backtracePath(  path );
           } else if (  _best_heuristic_node.first < getToleranceHeuristic(   ) ) {
           // Optimization: Let us find when in tolerance and refine within reason
           approach_iterations++;
           if (  approach_iterations >= getOnApproachMaxIterations(   ) ) {
           return _graph.at(  _best_heuristic_node.second ).backtracePath(  path );
           }
           }
          
           // 4 ) Expand neighbors of Nbest not visited
           neighbors.clear(   );
           current_node->getNeighbors(  neighborGetter,   _collision_checker,   _traverse_unknown,   neighbors );
          
           for (  neighbor_iterator = neighbors.begin(   );
           neighbor_iterator != neighbors.end(   ); ++neighbor_iterator )
           {
           neighbor = *neighbor_iterator;
          
           // 4.1 ) Compute the cost to go to this node
           g_cost = current_node->getAccumulatedCost(   ) + current_node->getTraversalCost(  neighbor );
          
           // 4.2 ) If this is a lower cost than prior,   we set this as the new cost and new approach
           if (  g_cost < neighbor->getAccumulatedCost(   ) ) {
           neighbor->setAccumulatedCost(  g_cost );
           neighbor->parent = current_node;
          
           // 4.3 ) Add to queue with heuristic cost
           addNode(  g_cost + getHeuristicCost(  neighbor ),   neighbor );
           }
           }
           }
          
           return false;
          }
          
          template<typename NodeT>
          bool AStarAlgorithm<NodeT>::isGoal(  NodePtr & node )
          {
           return node == getGoal(   );
          }
          
          template<typename NodeT>
          typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart(   )
          {
           return _start;
          }
          
          template<typename NodeT>
          typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal(   )
          {
           return _goal;
          }
          
          template<typename NodeT>
          typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode(   )
          {
           NodeBasic<NodeT> node = _queue.top(   ).second;
           _queue.pop(   );
           node.processSearchNode(   );
           return node.graph_node_ptr;
          }
          
          template<typename NodeT>
          void AStarAlgorithm<NodeT>::addNode(  const float & cost,   NodePtr & node )
          {
           NodeBasic<NodeT> queued_node(  node->getIndex(   ) );
           queued_node.populateSearchNode(  node );
           _queue.emplace(  cost,   queued_node );
          }
          
          template<typename NodeT>
          float AStarAlgorithm<NodeT>::getHeuristicCost(  const NodePtr & node )
          {
           const Coordinates node_coords =
           NodeT::getCoords(  node->getIndex(   ),   getSizeX(   ),   getSizeDim3(   ) );
           float heuristic = NodeT::getHeuristicCost(  
           node_coords,   _goal_coordinates,   _costmap );
          
           if (  heuristic < _best_heuristic_node.first ) {
           _best_heuristic_node = {heuristic,   node->getIndex(   )};
           }
          
           return heuristic;
          }
          
          template<typename NodeT>
          void AStarAlgorithm<NodeT>::clearQueue(   )
          {
           NodeQueue q;
           std::swap(  _queue,   q );
          }
          
          template<typename NodeT>
          void AStarAlgorithm<NodeT>::clearGraph(   )
          {
           Graph g;
           std::swap(  _graph,   g );
           _graph.reserve(  100000 );
          }
          
          template<typename NodeT>
          int & AStarAlgorithm<NodeT>::getMaxIterations(   )
          {
           return _max_iterations;
          }
          
          template<typename NodeT>
          int & AStarAlgorithm<NodeT>::getOnApproachMaxIterations(   )
          {
           return _max_on_approach_iterations;
          }
          
          template<typename NodeT>
          float & AStarAlgorithm<NodeT>::getToleranceHeuristic(   )
          {
           return _tolerance;
          }
          
          template<typename NodeT>
          unsigned int & AStarAlgorithm<NodeT>::getSizeX(   )
          {
           return _x_size;
          }
          
          template<typename NodeT>
          unsigned int & AStarAlgorithm<NodeT>::getSizeY(   )
          {
           return _y_size;
          }
          
          template<typename NodeT>
          unsigned int & AStarAlgorithm<NodeT>::getSizeDim3(   )
          {
           return _dim3_size;
          }
          
          // Instantiate algorithm for the supported template types
          template class AStarAlgorithm<Node2D>;
          template class AStarAlgorithm<NodeHybrid>;
          template class AStarAlgorithm<NodeLattice>;
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/analytic_expansion.cpp

          // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <ompl/base/ScopedState.h>
          #include <ompl/base/spaces/DubinsStateSpace.h>
          #include <ompl/base/spaces/ReedsSheppStateSpace.h>
          
          #include <algorithm>
          #include <vector>
          #include <memory>
          
          #include "nav2_smac_planner/analytic_expansion.hpp"
          
          namespace nav2_smac_planner
          {
          
          template<typename NodeT>
      29  AnalyticExpansion<NodeT>::AnalyticExpansion(  
      30   const MotionModel & motion_model,  
           const SearchInfo & search_info,  
      32   const bool & traverse_unknown,  
           const unsigned int & dim_3_size )
          : _motion_model(  motion_model ),  
           _search_info(  search_info ),  
           _traverse_unknown(  traverse_unknown ),  
           _dim_3_size(  dim_3_size ),  
           _collision_checker(  nullptr )
          {
          }
          
          template<typename NodeT>
      43  void AnalyticExpansion<NodeT>::setCollisionChecker(  
      44   GridCollisionChecker * collision_checker )
          {
           _collision_checker = collision_checker;
          }
          
          template<typename NodeT>
          typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(  
           const NodePtr & current_node,   const NodePtr & goal_node,  
           const NodeGetter & getter,   int & analytic_iterations,  
           int & closest_distance )
          {
           // This must be a valid motion model for analytic expansion to be attempted
      56   if (  _motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP ||
           _motion_model == MotionModel::STATE_LATTICE )
           {
           // See if we are closer and should be expanding more often
           auto costmap = _collision_checker->getCostmap(   );
           const Coordinates node_coords =
           NodeT::getCoords(  current_node->getIndex(   ),   costmap->getSizeInCellsX(   ),   _dim_3_size );
           closest_distance = std::min(  
           closest_distance,  
           static_cast<int>(  NodeT::getHeuristicCost(  node_coords,   goal_node->pose,   costmap ) ) );
          
           // We want to expand at a rate of d/expansion_ratio,  
           // but check to see if we are so close that we would be expanding every iteration
           // If so,   limit it to the expansion ratio (  rounded up )
           int desired_iterations = std::max(  
           static_cast<int>(  closest_distance / _search_info.analytic_expansion_ratio ),  
           static_cast<int>(  std::ceil(  _search_info.analytic_expansion_ratio ) ) );
          
           // If we are closer now,   we should update the target number of iterations to go
           analytic_iterations =
           std::min(  analytic_iterations,   desired_iterations );
          
           // Always run the expansion on the first run in case there is a
           // trivial path to be found
           if (  analytic_iterations <= 0 ) {
           // Reset the counter and try the analytic path expansion
           analytic_iterations = desired_iterations;
           AnalyticExpansionNodes analytic_nodes = getAnalyticPath(  current_node,   goal_node,   getter );
           if (  !analytic_nodes.empty(   ) ) {
           // If we have a valid path,   attempt to refine it
           NodePtr node = current_node;
           NodePtr test_node = current_node;
           AnalyticExpansionNodes refined_analytic_nodes;
           for (  int i = 0; i < 8; i++ ) {
           // Attempt to create better paths in 5 node increments,   need to make sure
           // they exist for each in order to do so (  maximum of 40 points back ).
           if (  test_node->parent && test_node->parent->parent && test_node->parent->parent->parent &&
           test_node->parent->parent->parent->parent &&
           test_node->parent->parent->parent->parent->parent )
           {
           test_node = test_node->parent->parent->parent->parent->parent;
           refined_analytic_nodes = getAnalyticPath(  test_node,   goal_node,   getter );
           if (  refined_analytic_nodes.empty(   ) ) {
           break;
           }
           analytic_nodes = refined_analytic_nodes;
           node = test_node;
           } else {
           break;
           }
           }
          
           return setAnalyticPath(  node,   goal_node,   analytic_nodes );
           }
           }
          
           analytic_iterations--;
           }
          
           // No valid motion model - return nullptr
     116   return NodePtr(  nullptr );
          }
          
          template<typename NodeT>
          typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(  
           const NodePtr & node,  
           const NodePtr & goal,  
           const NodeGetter & node_getter )
          {
     125   static ompl::base::ScopedState<> from(  node->motion_table.state_space ),   to(  
           node->motion_table.state_space ),   s(  node->motion_table.state_space );
     127   from[0] = node->pose.x;
     128   from[1] = node->pose.y;
           from[2] = node->motion_table.getAngleFromBin(  node->pose.theta );
           to[0] = goal->pose.x;
           to[1] = goal->pose.y;
           to[2] = node->motion_table.getAngleFromBin(  goal->pose.theta );
          
           float d = node->motion_table.state_space->distance(  from(   ),   to(   ) );
          
           // If the length is too far,   exit. This prevents unsafe shortcutting of paths
           // into higher cost areas far out from the goal itself,   let search to the work of getting
           // close before the analytic expansion brings it home. This should never be smaller than
           // 4-5x the minimum turning radius being used,   or planning times will begin to spike.
           if (  d > _search_info.analytic_expansion_max_length ) {
           return AnalyticExpansionNodes(   );
           }
          
           // A move of sqrt(  2 ) is guaranteed to be in a new cell
           static const float sqrt_2 = std::sqrt(  2. );
           unsigned int num_intervals = std::floor(  d / sqrt_2 );
          
           AnalyticExpansionNodes possible_nodes;
           // When "from" and "to" are zero or one cell away,  
           // num_intervals == 0
           possible_nodes.reserve(  num_intervals ); // We won't store this node or the goal
           std::vector<double> reals;
           double theta;
          
           // Pre-allocate
           NodePtr prev(  node );
           unsigned int index = 0;
           NodePtr next(  nullptr );
           float angle = 0.0;
           Coordinates proposed_coordinates;
           bool failure = false;
          
           // Check intermediary poses (  non-goal,   non-start )
           for (  float i = 1; i < num_intervals; i++ ) {
           node->motion_table.state_space->interpolate(  from(   ),   to(   ),   i / num_intervals,   s(   ) );
           reals = s.reals(   );
           // Make sure in range [0,   2PI )
           theta = (  reals[2] < 0.0 ) ? (  reals[2] + 2.0 * M_PI ) : reals[2];
           theta = (  theta > 2.0 * M_PI ) ? (  theta - 2.0 * M_PI ) : theta;
           angle = node->motion_table.getClosestAngularBin(  theta );
          
           // Turn the pose into a node,   and check if it is valid
           index = NodeT::getIndex(  
           static_cast<unsigned int>(  reals[0] ),  
           static_cast<unsigned int>(  reals[1] ),  
           static_cast<unsigned int>(  angle ) );
           // Get the node from the graph
           if (  node_getter(  index,   next ) ) {
           Coordinates initial_node_coords = next->pose;
           proposed_coordinates = {static_cast<float>(  reals[0] ),   static_cast<float>(  reals[1] ),   angle};
           next->setPose(  proposed_coordinates );
           if (  next->isNodeValid(  _traverse_unknown,   _collision_checker ) && next != prev ) {
           // Save the node,   and its previous coordinates in case we need to abort
           possible_nodes.emplace_back(  next,   initial_node_coords,   proposed_coordinates );
           prev = next;
           } else {
           // Abort
           next->setPose(  initial_node_coords );
           failure = true;
           break;
           }
           } else {
           // Abort
           failure = true;
           break;
           }
           }
          
           // Reset to initial poses to not impact future searches
           for (  const auto & node_pose : possible_nodes ) {
           const auto & n = node_pose.node;
           n->setPose(  node_pose.initial_coords );
           }
          
           if (  failure ) {
           return AnalyticExpansionNodes(   );
           }
          
           return possible_nodes;
          }
          
          template<typename NodeT>
          typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalyticPath(  
           const NodePtr & node,  
           const NodePtr & goal_node,  
           const AnalyticExpansionNodes & expanded_nodes )
          {
           _detached_nodes.clear(   );
           // Legitimate final path - set the parent relationships,   states,   and poses
           NodePtr prev = node;
           for (  const auto & node_pose : expanded_nodes ) {
           auto n = node_pose.node;
           cleanNode(  n );
           if (  n->getIndex(   ) != goal_node->getIndex(   ) ) {
           if (  n->wasVisited(   ) ) {
           _detached_nodes.push_back(  std::make_unique<NodeT>(  -1 ) );
           n = _detached_nodes.back(   ).get(   );
           }
           n->parent = prev;
           n->pose = node_pose.proposed_coords;
           n->visited(   );
           prev = n;
           }
           }
           if (  goal_node != prev ) {
           goal_node->parent = prev;
           cleanNode(  goal_node );
           goal_node->visited(   );
           }
           return goal_node;
          }
          
          template<>
          void AnalyticExpansion<NodeLattice>::cleanNode(  const NodePtr & node )
          {
           node->setMotionPrimitive(  nullptr );
          }
          
          template<typename NodeT>
          void AnalyticExpansion<NodeT>::cleanNode(  const NodePtr & /*expanded_nodes*/ )
          {
          }
          
          template<>
          typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Node2D>::
          getAnalyticPath(  
           const NodePtr & node,  
           const NodePtr & goal,  
           const NodeGetter & node_getter )
          {
           return AnalyticExpansionNodes(   );
          }
          
          template<>
          typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyticPath(  
           const NodePtr & node,  
           const NodePtr & goal_node,  
           const AnalyticExpansionNodes & expanded_nodes )
          {
           return NodePtr(  nullptr );
          }
          
          template<>
          typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(  
           const NodePtr & current_node,   const NodePtr & goal_node,  
           const NodeGetter & getter,   int & analytic_iterations,  
           int & closest_distance )
          {
           return NodePtr(  nullptr );
          }
          
          template class AnalyticExpansion<Node2D>;
          template class AnalyticExpansion<NodeHybrid>;
          template class AnalyticExpansion<NodeLattice>;
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/collision_checker.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include "nav2_smac_planner/collision_checker.hpp"
          
          namespace nav2_smac_planner
          {
          
      20  GridCollisionChecker::GridCollisionChecker(  
      21   nav2_costmap_2d::Costmap2D * costmap,  
           unsigned int num_quantizations )
          : FootprintCollisionChecker(  costmap )
          {
           // Convert number of regular bins into angles
           float bin_size = 2 * M_PI / static_cast<float>(  num_quantizations );
           angles_.reserve(  num_quantizations );
           for (  unsigned int i = 0; i != num_quantizations; i++ ) {
           angles_.push_back(  bin_size * i );
           }
          }
          
          // GridCollisionChecker::GridCollisionChecker(  
          // nav2_costmap_2d::Costmap2D * costmap,  
          // std::vector<float> & angles )
          // : FootprintCollisionChecker(  costmap ),  
          // angles_(  angles )
          // {
          // }
          
      41  void GridCollisionChecker::setFootprint(  
      42   const nav2_costmap_2d::Footprint & footprint,  
      43   const bool & radius,  
           const double & possible_inscribed_cost )
          {
           possible_inscribed_cost_ = possible_inscribed_cost;
           footprint_is_radius_ = radius;
          
           // Use radius,   no caching required
           if (  radius ) {
           return;
           }
          
           // No change,   no updates required
           if (  footprint == unoriented_footprint_ ) {
           return;
           }
          
           oriented_footprints_.reserve(  angles_.size(   ) );
           double sin_th,   cos_th;
           geometry_msgs::msg::Point new_pt;
           const unsigned int footprint_size = footprint.size(   );
          
           // Precompute the orientation bins for checking to use
           for (  unsigned int i = 0; i != angles_.size(   ); i++ ) {
           sin_th = sin(  angles_[i] );
           cos_th = cos(  angles_[i] );
           nav2_costmap_2d::Footprint oriented_footprint;
           oriented_footprint.reserve(  footprint_size );
          
           for (  unsigned int j = 0; j < footprint_size; j++ ) {
           new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th;
           new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th;
           oriented_footprint.push_back(  new_pt );
           }
          
           oriented_footprints_.push_back(  oriented_footprint );
           }
          
           unoriented_footprint_ = footprint;
          }
          
      83  bool GridCollisionChecker::inCollision(  
           const float & x,  
           const float & y,  
           const float & angle_bin,  
      87   const bool & traverse_unknown )
          {
           // Check to make sure cell is inside the map
           if (  outsideRange(  costmap_->getSizeInCellsX(   ),   x ) ||
           outsideRange(  costmap_->getSizeInCellsY(   ),   y ) )
           {
           return false;
           }
          
           // Assumes setFootprint already set
           double wx,   wy;
           costmap_->mapToWorld(  static_cast<double>(  x ),   static_cast<double>(  y ),   wx,   wy );
          
           if (  !footprint_is_radius_ ) {
           // if footprint,   then we check for the footprint's points,   but first see
           // if the robot is even potentially in an inscribed collision
           footprint_cost_ = costmap_->getCost(  
           static_cast<unsigned int>(  x ),   static_cast<unsigned int>(  y ) );
          
           if (  footprint_cost_ < possible_inscribed_cost_ ) {
           return false;
           }
          
           // If its inscribed,   in collision,   or unknown in the middle,  
           // no need to even check the footprint,   its invalid
           if (  footprint_cost_ == UNKNOWN && !traverse_unknown ) {
           return true;
           }
          
           if (  footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED ) {
           return true;
           }
          
           // if possible inscribed,   need to check actual footprint pose.
           // Use precomputed oriented footprints are done on initialization,  
           // offset by translation value to collision check
           geometry_msgs::msg::Point new_pt;
           const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin];
           nav2_costmap_2d::Footprint current_footprint;
           current_footprint.reserve(  oriented_footprint.size(   ) );
           for (  unsigned int i = 0; i < oriented_footprint.size(   ); ++i ) {
           new_pt.x = wx + oriented_footprint[i].x;
           new_pt.y = wy + oriented_footprint[i].y;
           current_footprint.push_back(  new_pt );
           }
          
           footprint_cost_ = footprintCost(  current_footprint );
          
           if (  footprint_cost_ == UNKNOWN && traverse_unknown ) {
           return false;
           }
          
           // if occupied or unknown and not to traverse unknown space
           return footprint_cost_ >= OCCUPIED;
           } else {
           // if radius,   then we can check the center of the cost assuming inflation is used
           footprint_cost_ = costmap_->getCost(  
           static_cast<unsigned int>(  x ),   static_cast<unsigned int>(  y ) );
          
           if (  footprint_cost_ == UNKNOWN && traverse_unknown ) {
           return false;
           }
          
           // if occupied or unknown and not to traverse unknown space
           return static_cast<double>(  footprint_cost_ ) >= INSCRIBED;
           }
          }
          
     155  bool GridCollisionChecker::inCollision(  
           const unsigned int & i,  
     157   const bool & traverse_unknown )
          {
           footprint_cost_ = costmap_->getCost(  i );
           if (  footprint_cost_ == UNKNOWN && traverse_unknown ) {
           return false;
           }
          
           // if occupied or unknown and not to traverse unknown space
           return footprint_cost_ >= INSCRIBED;
          }
          
     168  float GridCollisionChecker::getCost(   )
          {
           // Assumes inCollision called prior
           return static_cast<float>(  footprint_cost_ );
          }
          
     174  bool GridCollisionChecker::outsideRange(  const unsigned int & max,   const float & value )
          {
           return value < 0.0f || value > max;
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/costmap_downsampler.cpp

       1  // Copyright (  c ) 2020,   Carlos Luis
          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include "nav2_smac_planner/costmap_downsampler.hpp"
          
          #include <string>
          #include <memory>
          #include <algorithm>
          
          namespace nav2_smac_planner
          {
          
      25  CostmapDownsampler::CostmapDownsampler(   )
          : _costmap(  nullptr ),  
           _downsampled_costmap(  nullptr ),  
           _downsampled_costmap_pub(  nullptr )
          {
          }
          
      32  CostmapDownsampler::~CostmapDownsampler(   )
          {
          }
          
      36  void CostmapDownsampler::on_configure(  
           const nav2_util::LifecycleNode::WeakPtr & node,  
           const std::string & global_frame,  
           const std::string & topic_name,  
           nav2_costmap_2d::Costmap2D * const costmap,  
           const unsigned int & downsampling_factor,  
           const bool & use_min_cost_neighbor )
          {
           _costmap = costmap;
           _downsampling_factor = downsampling_factor;
           _use_min_cost_neighbor = use_min_cost_neighbor;
           updateCostmapSize(   );
          
           _downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>(  
           _downsampled_size_x,   _downsampled_size_y,   _downsampled_resolution,  
           _costmap->getOriginX(   ),   _costmap->getOriginY(   ),   UNKNOWN );
          
           if (  !node.expired(   ) ) {
           _downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(  
           node,   _downsampled_costmap.get(   ),   global_frame,   topic_name,   false );
           }
          }
          
      59  void CostmapDownsampler::on_activate(   )
          {
           if (  _downsampled_costmap_pub ) {
           _downsampled_costmap_pub->on_activate(   );
           }
          }
          
      66  void CostmapDownsampler::on_deactivate(   )
          {
           if (  _downsampled_costmap_pub ) {
           _downsampled_costmap_pub->on_deactivate(   );
           }
          }
          
      73  void CostmapDownsampler::on_cleanup(   )
          {
           _costmap = nullptr;
           _downsampled_costmap.reset(   );
           _downsampled_costmap_pub.reset(   );
          }
          
      80  nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(  
           const unsigned int & downsampling_factor )
          {
           _downsampling_factor = downsampling_factor;
           updateCostmapSize(   );
          
           // Adjust costmap size if needed
           if (  _downsampled_costmap->getSizeInCellsX(   ) != _downsampled_size_x ||
           _downsampled_costmap->getSizeInCellsY(   ) != _downsampled_size_y ||
           _downsampled_costmap->getResolution(   ) != _downsampled_resolution )
           {
           resizeCostmap(   );
           }
          
           // Assign costs
           for (  unsigned int i = 0; i < _downsampled_size_x; ++i ) {
           for (  unsigned int j = 0; j < _downsampled_size_y; ++j ) {
           setCostOfCell(  i,   j );
           }
           }
          
           if (  _downsampled_costmap_pub ) {
           _downsampled_costmap_pub->publishCostmap(   );
           }
           return _downsampled_costmap.get(   );
          }
          
     107  void CostmapDownsampler::updateCostmapSize(   )
          {
           _size_x = _costmap->getSizeInCellsX(   );
           _size_y = _costmap->getSizeInCellsY(   );
           _downsampled_size_x = ceil(  static_cast<float>(  _size_x ) / _downsampling_factor );
           _downsampled_size_y = ceil(  static_cast<float>(  _size_y ) / _downsampling_factor );
           _downsampled_resolution = _downsampling_factor * _costmap->getResolution(   );
          }
          
     116  void CostmapDownsampler::resizeCostmap(   )
          {
           _downsampled_costmap->resizeMap(  
           _downsampled_size_x,  
           _downsampled_size_y,  
           _downsampled_resolution,  
           _costmap->getOriginX(   ),  
           _costmap->getOriginY(   ) );
          }
          
     126  void CostmapDownsampler::setCostOfCell(  
           const unsigned int & new_mx,  
           const unsigned int & new_my )
          {
           unsigned int mx,   my;
           unsigned char cost = _use_min_cost_neighbor ? 255 : 0;
           unsigned int x_offset = new_mx * _downsampling_factor;
           unsigned int y_offset = new_my * _downsampling_factor;
          
           for (  unsigned int i = 0; i < _downsampling_factor; ++i ) {
           mx = x_offset + i;
           if (  mx >= _size_x ) {
           continue;
           }
           for (  unsigned int j = 0; j < _downsampling_factor; ++j ) {
           my = y_offset + j;
           if (  my >= _size_y ) {
           continue;
           }
           cost = _use_min_cost_neighbor ?
           std::min(  cost,   _costmap->getCost(  mx,   my ) ) :
           std::max(  cost,   _costmap->getCost(  mx,   my ) );
           }
           }
          
           _downsampled_costmap->setCost(  new_mx,   new_my,   cost );
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_2d.cpp

          // Copyright (  c ) 2020,   Samsung Research America
          // Copyright (  c ) 2020,   Applied Electric Vehicles Pty Ltd
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include "nav2_smac_planner/node_2d.hpp"
          
          #include <vector>
          #include <limits>
          
          namespace nav2_smac_planner
          {
          
          // defining static member for all instance to share
          std::vector<int> Node2D::_neighbors_grid_offsets;
          float Node2D::cost_travel_multiplier = 2.0;
          
      28  Node2D::Node2D(  const unsigned int index )
          : parent(  nullptr ),  
           _cell_cost(  std::numeric_limits<float>::quiet_NaN(   ) ),  
           _accumulated_cost(  std::numeric_limits<float>::max(   ) ),  
           _index(  index ),  
           _was_visited(  false ),  
           _is_queued(  false )
          {
          }
          
          Node2D::~Node2D(   )
          {
           parent = nullptr;
          }
          
          void Node2D::reset(   )
          {
           parent = nullptr;
           _cell_cost = std::numeric_limits<float>::quiet_NaN(   );
           _accumulated_cost = std::numeric_limits<float>::max(   );
           _was_visited = false;
           _is_queued = false;
          }
          
          bool Node2D::isNodeValid(  
           const bool & traverse_unknown,  
           GridCollisionChecker * collision_checker )
          {
           if (  collision_checker->inCollision(  this->getIndex(   ),   traverse_unknown ) ) {
           return false;
           }
          
           _cell_cost = collision_checker->getCost(   );
           return true;
          }
          
          float Node2D::getTraversalCost(  const NodePtr & child )
          {
           float normalized_cost = child->getCost(   ) / 252.0;
           const Coordinates A = getCoords(  child->getIndex(   ) );
           const Coordinates B = getCoords(  this->getIndex(   ) );
           const float & dx = A.x - B.x;
           const float & dy = A.y - B.y;
           static float sqrt_2 = sqrt(  2 );
          
           // If a diagonal move,   travel cost is sqrt(  2 ) not 1.0.
           if (  (  dx * dx + dy * dy ) > 1.05 ) {
           return sqrt_2 + cost_travel_multiplier * normalized_cost;
           }
          
           return 1.0 + cost_travel_multiplier * normalized_cost;
          }
          
          float Node2D::getHeuristicCost(  
           const Coordinates & node_coords,  
           const Coordinates & goal_coordinates,  
           const nav2_costmap_2d::Costmap2D * /*costmap*/ )
          {
           // Using Moore distance as it more accurately represents the distances
           // even a Van Neumann neighborhood robot can navigate.
           return fabs(  goal_coordinates.x - node_coords.x ) +
           fabs(  goal_coordinates.y - node_coords.y );
          }
          
          void Node2D::initMotionModel(  
           const MotionModel & neighborhood,  
           unsigned int & x_size_uint,  
           unsigned int & /*size_y*/,  
           unsigned int & /*num_angle_quantization*/,  
           SearchInfo & search_info )
          {
           int x_size = static_cast<int>(  x_size_uint );
           cost_travel_multiplier = search_info.cost_penalty;
           switch (  neighborhood ) {
           case MotionModel::UNKNOWN:
           throw std::runtime_error(  "Unknown neighborhood type selected." );
           case MotionModel::VON_NEUMANN:
           _neighbors_grid_offsets = {-1,   +1,   -x_size,   +x_size};
           break;
           case MotionModel::MOORE:
           _neighbors_grid_offsets = {-1,   +1,   -x_size,   +x_size,   -x_size - 1,  
           -x_size + 1,   +x_size - 1,   +x_size + 1};
           break;
           default:
           throw std::runtime_error(  
           "Invalid neighborhood type selected. "
           "Von-Neumann and Moore are valid for Node2D." );
           }
          }
          
          void Node2D::getNeighbors(  
           std::function<bool(  const unsigned int &,   nav2_smac_planner::Node2D * & )> & NeighborGetter,  
           GridCollisionChecker * collision_checker,  
           const bool & traverse_unknown,  
           NodeVector & neighbors )
          {
           // NOTE(  stevemacenski ): Irritatingly,   the order here matters. If you start in free
           // space and then expand 8-connected,   the first set of neighbors will be all cost
           // 1.0. Then its expansion will all be 2 * 1.0 but now multiple
           // nodes are touching that node so the last cell to update the back pointer wins.
           // Thusly,   the ordering ends with the cardinal directions for both sets such that
           // behavior is consistent in large free spaces between them.
           // 100 50 0
           // 100 50 50
           // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes
           // Therefore,   it is valuable to have some low-potential across the entire map
           // rather than a small inflation around the obstacles
           int index;
           NodePtr neighbor;
           int node_i = this->getIndex(   );
           const Coordinates parent = getCoords(  this->getIndex(   ) );
           Coordinates child;
          
           for (  unsigned int i = 0; i != _neighbors_grid_offsets.size(   ); ++i ) {
           index = node_i + _neighbors_grid_offsets[i];
          
           // Check for wrap around conditions
           child = getCoords(  index );
           if (  fabs(  parent.x - child.x ) > 1 || fabs(  parent.y - child.y ) > 1 ) {
           continue;
           }
          
           if (  NeighborGetter(  index,   neighbor ) ) {
           if (  neighbor->isNodeValid(  traverse_unknown,   collision_checker ) && !neighbor->wasVisited(   ) ) {
           neighbors.push_back(  neighbor );
           }
           }
           }
          }
          
          bool Node2D::backtracePath(  CoordinateVector & path )
          {
           if (  !this->parent ) {
           return false;
           }
          
           NodePtr current_node = this;
          
           while (  current_node->parent ) {
           path.push_back(  
           Node2D::getCoords(  current_node->getIndex(   ) ) );
           current_node = current_node->parent;
           }
          
           return path.size(   ) > 0;
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_basic.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include "nav2_smac_planner/node_basic.hpp"
          
          namespace nav2_smac_planner
          {
          
          template<typename Node2D>
      21  void NodeBasic<Node2D>::processSearchNode(   )
          {
          }
          
          template<>
      26  void NodeBasic<NodeHybrid>::processSearchNode(   )
          {
           // We only want to override the node's pose if it has not yet been visited
           // to prevent the case that a node has been queued multiple times and
           // a new branch is overriding one of lower cost already visited.
           if (  !this->graph_node_ptr->wasVisited(   ) ) {
           this->graph_node_ptr->pose = this->pose;
           this->graph_node_ptr->setMotionPrimitiveIndex(  this->motion_index );
           }
          }
          
          template<>
      38  void NodeBasic<NodeLattice>::processSearchNode(   )
          {
           // We only want to override the node's pose/primitive if it has not yet been visited
           // to prevent the case that a node has been queued multiple times and
           // a new branch is overriding one of lower cost already visited.
           if (  !this->graph_node_ptr->wasVisited(   ) ) {
           this->graph_node_ptr->pose = this->pose;
           this->graph_node_ptr->setMotionPrimitive(  this->prim_ptr );
           this->graph_node_ptr->backwards(  this->backward );
           }
          }
          
          template<>
      51  void NodeBasic<Node2D>::populateSearchNode(  Node2D * & node )
          {
           this->graph_node_ptr = node;
          }
          
          template<>
      57  void NodeBasic<NodeHybrid>::populateSearchNode(  NodeHybrid * & node )
          {
           this->pose = node->pose;
           this->graph_node_ptr = node;
           this->motion_index = node->getMotionPrimitiveIndex(   );
          }
          
          template<>
      65  void NodeBasic<NodeLattice>::populateSearchNode(  NodeLattice * & node )
          {
           this->pose = node->pose;
           this->graph_node_ptr = node;
           this->prim_ptr = node->getMotionPrimitive(   );
           this->backward = node->isBackward(   );
          }
          
      73  template class NodeBasic<Node2D>;
      74  template class NodeBasic<NodeHybrid>;
      75  template class NodeBasic<NodeLattice>;
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_hybrid.cpp

          // Copyright (  c ) 2020,   Samsung Research America
          // Copyright (  c ) 2020,   Applied Electric Vehicles Pty Ltd
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <chrono>
          #include <vector>
          #include <memory>
          #include <algorithm>
          #include <queue>
          #include <limits>
          #include <utility>
          
          #include "ompl/base/ScopedState.h"
          #include "ompl/base/spaces/DubinsStateSpace.h"
          #include "ompl/base/spaces/ReedsSheppStateSpace.h"
          
          #include "nav2_smac_planner/node_hybrid.hpp"
          
          using namespace std::chrono; // NOLINT
          
          namespace nav2_smac_planner
          {
          
          // defining static member for all instance to share
          LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
          double NodeHybrid::travel_distance_cost = sqrt(  2 );
          HybridMotionTable NodeHybrid::motion_table;
          float NodeHybrid::size_lookup = 25;
          LookupTable NodeHybrid::dist_heuristic_lookup_table;
          nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
          CostmapDownsampler NodeHybrid::downsampler;
          ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
          
          // Each of these tables are the projected motion models through
          // time and space applied to the search on the current node in
          // continuous map-coordinates (  e.g. not meters but partial map cells )
          // Currently,   these are set to project *at minimum* into a neighboring
          // cell. Though this could be later modified to project a certain
          // amount of time or particular distance forward.
          
          // http://planning.cs.uiuc.edu/node821.html
          // Model for ackermann style vehicle with minimum radius restriction
          void HybridMotionTable::initDubin(  
           unsigned int & size_x_in,  
           unsigned int & /*size_y_in*/,  
           unsigned int & num_angle_quantization_in,  
           SearchInfo & search_info )
          {
           size_x = size_x_in;
           change_penalty = search_info.change_penalty;
           non_straight_penalty = search_info.non_straight_penalty;
           cost_penalty = search_info.cost_penalty;
           reverse_penalty = search_info.reverse_penalty;
           travel_distance_reward = 1.0f - search_info.retrospective_penalty;
          
           // if nothing changed,   no need to re-compute primitives
           if (  num_angle_quantization_in == num_angle_quantization &&
           min_turning_radius == search_info.minimum_turning_radius &&
           motion_model == MotionModel::DUBIN )
           {
           return;
           }
          
           num_angle_quantization = num_angle_quantization_in;
           num_angle_quantization_float = static_cast<float>(  num_angle_quantization );
           min_turning_radius = search_info.minimum_turning_radius;
           motion_model = MotionModel::DUBIN;
          
           // angle must meet 3 requirements:
           // 1 ) be increment of quantized bin size
           // 2 ) chord length must be greater than sqrt(  2 ) to leave current cell
           // 3 ) maximum curvature must be respected,   represented by minimum turning angle
           // Thusly:
           // On circle of radius minimum turning angle,   we need select motion primatives
           // with chord length > sqrt(  2 ) and be an increment of our bin size
           //
           // chord >= sqrt(  2 ) >= 2 * R * sin (  angle / 2 ); where angle / N = quantized bin size
           // Thusly: angle <= 2.0 * asin(  sqrt(  2 ) / (  2 * R ) )
           float angle = 2.0 * asin(  sqrt(  2.0 ) / (  2 * min_turning_radius ) );
           // Now make sure angle is an increment of the quantized bin size
           // And since its based on the minimum chord,   we need to make sure its always larger
           bin_size =
           2.0f * static_cast<float>(  M_PI ) / static_cast<float>(  num_angle_quantization );
           float increments;
           if (  angle < bin_size ) {
           increments = 1.0f;
           } else {
           // Search dimensions are clean multiples of quantization - this prevents
           // paths with loops in them
           increments = ceil(  angle / bin_size );
           }
           angle = increments * bin_size;
          
           // find deflections
           // If we make a right triangle out of the chord in circle of radius
           // min turning angle,   we can see that delta X = R * sin (  angle )
           float delta_x = min_turning_radius * sin(  angle );
           // Using that same right triangle,   we can see that the complement
           // to delta Y is R * cos (  angle ). If we subtract R,   we get the actual value
           float delta_y = min_turning_radius - (  min_turning_radius * cos(  angle ) );
          
           projections.clear(   );
           projections.reserve(  3 );
           projections.emplace_back(  hypotf(  delta_x,   delta_y ),   0.0,   0.0 ); // Forward
           projections.emplace_back(  delta_x,   delta_y,   increments ); // Left
           projections.emplace_back(  delta_x,   -delta_y,   -increments ); // Right
          
           // Create the correct OMPL state space
           state_space = std::make_unique<ompl::base::DubinsStateSpace>(  min_turning_radius );
          
           // Precompute projection deltas
           delta_xs.resize(  projections.size(   ) );
           delta_ys.resize(  projections.size(   ) );
           trig_values.resize(  num_angle_quantization );
          
           for (  unsigned int i = 0; i != projections.size(   ); i++ ) {
           delta_xs[i].resize(  num_angle_quantization );
           delta_ys[i].resize(  num_angle_quantization );
          
           for (  unsigned int j = 0; j != num_angle_quantization; j++ ) {
           double cos_theta = cos(  bin_size * j );
           double sin_theta = sin(  bin_size * j );
           if (  i == 0 ) {
           // if first iteration,   cache the trig values for later
           trig_values[j] = {cos_theta,   sin_theta};
           }
           delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
           delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
           }
           }
          }
          
          // http://planning.cs.uiuc.edu/node822.html
          // Same as Dubin model but now reverse is valid
          // See notes in Dubin for explanation
          void HybridMotionTable::initReedsShepp(  
           unsigned int & size_x_in,  
           unsigned int & /*size_y_in*/,  
           unsigned int & num_angle_quantization_in,  
           SearchInfo & search_info )
          {
           size_x = size_x_in;
           change_penalty = search_info.change_penalty;
           non_straight_penalty = search_info.non_straight_penalty;
           cost_penalty = search_info.cost_penalty;
           reverse_penalty = search_info.reverse_penalty;
           travel_distance_reward = 1.0f - search_info.retrospective_penalty;
          
           // if nothing changed,   no need to re-compute primitives
           if (  num_angle_quantization_in == num_angle_quantization &&
           min_turning_radius == search_info.minimum_turning_radius &&
           motion_model == MotionModel::REEDS_SHEPP )
           {
           return;
           }
          
           num_angle_quantization = num_angle_quantization_in;
           num_angle_quantization_float = static_cast<float>(  num_angle_quantization );
           min_turning_radius = search_info.minimum_turning_radius;
           motion_model = MotionModel::REEDS_SHEPP;
          
           float angle = 2.0 * asin(  sqrt(  2.0 ) / (  2 * min_turning_radius ) );
           bin_size =
           2.0f * static_cast<float>(  M_PI ) / static_cast<float>(  num_angle_quantization );
           float increments;
           if (  angle < bin_size ) {
           increments = 1.0f;
           } else {
           increments = ceil(  angle / bin_size );
           }
           angle = increments * bin_size;
          
           float delta_x = min_turning_radius * sin(  angle );
           float delta_y = min_turning_radius - (  min_turning_radius * cos(  angle ) );
          
           projections.clear(   );
           projections.reserve(  6 );
           projections.emplace_back(  hypotf(  delta_x,   delta_y ),   0.0,   0.0 ); // Forward
           projections.emplace_back(  delta_x,   delta_y,   increments ); // Forward + Left
           projections.emplace_back(  delta_x,   -delta_y,   -increments ); // Forward + Right
           projections.emplace_back(  -hypotf(  delta_x,   delta_y ),   0.0,   0.0 ); // Backward
           projections.emplace_back(  -delta_x,   delta_y,   -increments ); // Backward + Left
           projections.emplace_back(  -delta_x,   -delta_y,   increments ); // Backward + Right
          
           // Create the correct OMPL state space
           state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(  min_turning_radius );
          
           // Precompute projection deltas
           delta_xs.resize(  projections.size(   ) );
           delta_ys.resize(  projections.size(   ) );
           trig_values.resize(  num_angle_quantization );
          
           for (  unsigned int i = 0; i != projections.size(   ); i++ ) {
           delta_xs[i].resize(  num_angle_quantization );
           delta_ys[i].resize(  num_angle_quantization );
          
           for (  unsigned int j = 0; j != num_angle_quantization; j++ ) {
           double cos_theta = cos(  bin_size * j );
           double sin_theta = sin(  bin_size * j );
           if (  i == 0 ) {
           // if first iteration,   cache the trig values for later
           trig_values[j] = {cos_theta,   sin_theta};
           }
           delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
           delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
           }
           }
          }
          
          MotionPoses HybridMotionTable::getProjections(  const NodeHybrid * node )
          {
           MotionPoses projection_list;
           projection_list.reserve(  projections.size(   ) );
          
           for (  unsigned int i = 0; i != projections.size(   ); i++ ) {
           const MotionPose & motion_model = projections[i];
          
           // normalize theta,   I know its overkill,   but I've been burned before...
           const float & node_heading = node->pose.theta;
           float new_heading = node_heading + motion_model._theta;
          
           if (  new_heading < 0.0 ) {
           new_heading += num_angle_quantization_float;
           }
          
           if (  new_heading >= num_angle_quantization_float ) {
           new_heading -= num_angle_quantization_float;
           }
          
           projection_list.emplace_back(  
           delta_xs[i][node_heading] + node->pose.x,  
           delta_ys[i][node_heading] + node->pose.y,  
           new_heading );
           }
          
           return projection_list;
          }
          
          unsigned int HybridMotionTable::getClosestAngularBin(  const double & theta )
          {
           return static_cast<unsigned int>(  floor(  theta / bin_size ) );
          }
          
          float HybridMotionTable::getAngleFromBin(  const unsigned int & bin_idx )
          {
           return bin_idx * bin_size;
          }
          
     261  NodeHybrid::NodeHybrid(  const unsigned int index )
          : parent(  nullptr ),  
     263   pose(  0.0f,   0.0f,   0.0f ),  
           _cell_cost(  std::numeric_limits<float>::quiet_NaN(   ) ),  
           _accumulated_cost(  std::numeric_limits<float>::max(   ) ),  
           _index(  index ),  
           _was_visited(  false ),  
           _motion_primitive_index(  std::numeric_limits<unsigned int>::max(   ) )
          {
          }
          
     272  NodeHybrid::~NodeHybrid(   )
          {
           parent = nullptr;
          }
          
     277  void NodeHybrid::reset(   )
          {
           parent = nullptr;
           _cell_cost = std::numeric_limits<float>::quiet_NaN(   );
           _accumulated_cost = std::numeric_limits<float>::max(   );
           _was_visited = false;
           _motion_primitive_index = std::numeric_limits<unsigned int>::max(   );
           pose.x = 0.0f;
           pose.y = 0.0f;
           pose.theta = 0.0f;
          }
          
     289  bool NodeHybrid::isNodeValid(  
     290   const bool & traverse_unknown,  
     291   GridCollisionChecker * collision_checker )
          {
           if (  collision_checker->inCollision(  
           this->pose.x,   this->pose.y,   this->pose.theta /*bin number*/,   traverse_unknown ) )
           {
           return false;
           }
          
           _cell_cost = collision_checker->getCost(   );
           return true;
          }
          
     303  float NodeHybrid::getTraversalCost(  const NodePtr & child )
          {
           const float normalized_cost = child->getCost(   ) / 252.0;
           if (  std::isnan(  normalized_cost ) ) {
           throw std::runtime_error(  
           "Node attempted to get traversal "
           "cost without a known SE2 collision cost!" );
           }
          
           // this is the first node
           if (  getMotionPrimitiveIndex(   ) == std::numeric_limits<unsigned int>::max(   ) ) {
           return NodeHybrid::travel_distance_cost;
           }
          
           float travel_cost = 0.0;
           float travel_cost_raw =
           NodeHybrid::travel_distance_cost *
           (  motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost );
          
           if (  child->getMotionPrimitiveIndex(   ) == 0 || child->getMotionPrimitiveIndex(   ) == 3 ) {
           // New motion is a straight motion,   no additional costs to be applied
           travel_cost = travel_cost_raw;
           } else {
           if (  getMotionPrimitiveIndex(   ) == child->getMotionPrimitiveIndex(   ) ) {
           // Turning motion but keeps in same direction: encourages to commit to turning if starting it
           travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
           } else {
           // Turning motion and changing direction: penalizes wiggling
           travel_cost = travel_cost_raw *
           (  motion_table.non_straight_penalty + motion_table.change_penalty );
           }
           }
          
           if (  child->getMotionPrimitiveIndex(   ) > 2 ) {
           // reverse direction
           travel_cost *= motion_table.reverse_penalty;
           }
          
           return travel_cost;
          }
          
     344  float NodeHybrid::getHeuristicCost(  
     345   const Coordinates & node_coords,  
     346   const Coordinates & goal_coords,  
     347   const nav2_costmap_2d::Costmap2D * /*costmap*/ )
          {
           const float obstacle_heuristic =
           getObstacleHeuristic(  node_coords,   goal_coords,   motion_table.cost_penalty );
           const float dist_heuristic = getDistanceHeuristic(  node_coords,   goal_coords,   obstacle_heuristic );
           return std::max(  obstacle_heuristic,   dist_heuristic );
          }
          
     355  void NodeHybrid::initMotionModel(  
     356   const MotionModel & motion_model,  
           unsigned int & size_x,  
           unsigned int & size_y,  
           unsigned int & num_angle_quantization,  
           SearchInfo & search_info )
          {
           // find the motion model selected
           switch (  motion_model ) {
           case MotionModel::DUBIN:
           motion_table.initDubin(  size_x,   size_y,   num_angle_quantization,   search_info );
           break;
           case MotionModel::REEDS_SHEPP:
           motion_table.initReedsShepp(  size_x,   size_y,   num_angle_quantization,   search_info );
           break;
           default:
           throw std::runtime_error(  
           "Invalid motion model for Hybrid A*. Please select between"
           " Dubin (  Ackermann forward only ),  "
           " Reeds-Shepp (  Ackermann forward and back )." );
           }
          
           travel_distance_cost = motion_table.projections[0]._x;
          }
          
     380  inline float distanceHeuristic2D(  
           const unsigned int idx,   const unsigned int size_x,  
           const unsigned int target_x,   const unsigned int target_y )
          {
           return std::hypotf(  
           static_cast<int>(  idx % size_x ) - static_cast<int>(  target_x ),  
           static_cast<int>(  idx / size_x ) - static_cast<int>(  target_y ) );
          }
          
     389  void NodeHybrid::resetObstacleHeuristic(  
     390   nav2_costmap_2d::Costmap2D * costmap,  
           const unsigned int & start_x,   const unsigned int & start_y,  
           const unsigned int & goal_x,   const unsigned int & goal_y )
          {
           // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up
           // the planner considerably to search through 75% less cells with no detectable
           // erosion of path quality after even modest smoothing. The error would be no more
           // than 0.05 * normalized cost. Since this is just a search prior,   there's no loss in generality
           std::weak_ptr<nav2_util::LifecycleNode> ptr;
           downsampler.on_configure(  ptr,   "fake_frame",   "fake_topic",   costmap,   2.0,   true );
           downsampler.on_activate(   );
           sampled_costmap = downsampler.downsample(  2.0 );
          
           // Clear lookup table
           unsigned int size = sampled_costmap->getSizeInCellsX(   ) * sampled_costmap->getSizeInCellsY(   );
           if (  obstacle_heuristic_lookup_table.size(   ) == size ) {
           // must reset all values
           std::fill(  
           obstacle_heuristic_lookup_table.begin(   ),  
           obstacle_heuristic_lookup_table.end(   ),   0.0 );
           } else {
           unsigned int obstacle_size = obstacle_heuristic_lookup_table.size(   );
           obstacle_heuristic_lookup_table.resize(  size,   0.0 );
           // must reset values for non-constructed indices
           std::fill_n(  
           obstacle_heuristic_lookup_table.begin(   ),   obstacle_size,   0.0 );
           }
          
           obstacle_heuristic_queue.clear(   );
           obstacle_heuristic_queue.reserve(  
           sampled_costmap->getSizeInCellsX(   ) * sampled_costmap->getSizeInCellsY(   ) );
          
           // Set initial goal point to queue from. Divided by 2 due to downsampled costmap.
           const unsigned int size_x = sampled_costmap->getSizeInCellsX(   );
           const unsigned int goal_index = floor(  goal_y / 2.0 ) * size_x + floor(  goal_x / 2.0 );
           obstacle_heuristic_queue.emplace_back(  
           distanceHeuristic2D(  goal_index,   size_x,   start_x,   start_y ),   goal_index );
          
           // initialize goal cell with a very small value to differentiate it from 0.0 (  ~uninitialized )
           // the negative value means the cell is in the open set
           obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
          }
          
     433  float NodeHybrid::getObstacleHeuristic(  
     434   const Coordinates & node_coords,  
     435   const Coordinates & goal_coords,  
           const double & cost_penalty )
          {
           // If already expanded,   return the cost
           const unsigned int size_x = sampled_costmap->getSizeInCellsX(   );
           // Divided by 2 due to downsampled costmap.
           const unsigned int start_y = floor(  node_coords.y / 2.0 );
           const unsigned int start_x = floor(  node_coords.x / 2.0 );
           const unsigned int start_index = start_y * size_x + start_x;
           const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index];
           if (  requested_node_cost > 0.0f ) {
           // costs are doubled due to downsampling
           return 2.0 * requested_node_cost;
           }
          
           // If not,   expand until it is included. This dynamic programming ensures that
           // we only expand the MINIMUM spanning set of the costmap per planning request.
           // Rather than naively expanding the entire (  potentially massive ) map for a limited
           // path,   we only expand to the extent required for the furthest expansion in the
           // search-planning request that dynamically updates during search as needed.
          
           // start_x and start_y have changed since last call
           // we need to recompute 2D distance heuristic and reprioritize queue
           for (  auto & n : obstacle_heuristic_queue ) {
           n.first = -obstacle_heuristic_lookup_table[n.second] +
           distanceHeuristic2D(  n.second,   size_x,   start_x,   start_y );
           }
           std::make_heap(  
           obstacle_heuristic_queue.begin(   ),   obstacle_heuristic_queue.end(   ),  
           ObstacleHeuristicComparator{} );
          
           const int size_x_int = static_cast<int>(  size_x );
           const unsigned int size_y = sampled_costmap->getSizeInCellsY(   );
           const float sqrt_2 = sqrt(  2 );
           float c_cost,   cost,   travel_cost,   new_cost,   existing_cost;
           unsigned int idx,   mx,   my,   mx_idx,   my_idx;
           unsigned int new_idx = 0;
          
           const std::vector<int> neighborhood = {1,   -1,   // left right
           size_x_int,   -size_x_int,   // up down
           size_x_int + 1,   size_x_int - 1,   // upper diagonals
           -size_x_int + 1,   -size_x_int - 1}; // lower diagonals
          
           while (  !obstacle_heuristic_queue.empty(   ) ) {
           idx = obstacle_heuristic_queue.front(   ).second;
           std::pop_heap(  
           obstacle_heuristic_queue.begin(   ),   obstacle_heuristic_queue.end(   ),  
           ObstacleHeuristicComparator{} );
           obstacle_heuristic_queue.pop_back(   );
           c_cost = obstacle_heuristic_lookup_table[idx];
           if (  c_cost > 0.0f ) {
           // cell has been processed and closed,   no further cost improvements
           // are mathematically possible thanks to euclidean distance heuristic consistency
           continue;
           }
           c_cost = -c_cost;
           obstacle_heuristic_lookup_table[idx] = c_cost; // set a positive value to close the cell
          
           my_idx = idx / size_x;
           mx_idx = idx - (  my_idx * size_x );
          
           // find neighbors
           for (  unsigned int i = 0; i != neighborhood.size(   ); i++ ) {
           new_idx = static_cast<unsigned int>(  static_cast<int>(  idx ) + neighborhood[i] );
          
           // if neighbor path is better and non-lethal,   set new cost and add to queue
           if (  new_idx < size_x * size_y ) {
           cost = static_cast<float>(  sampled_costmap->getCost(  new_idx ) );
           if (  cost >= INSCRIBED ) {
           continue;
           }
          
           my = new_idx / size_x;
           mx = new_idx - (  my * size_x );
          
           if (  mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0 ) {
           continue;
           }
           if (  my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0 ) {
           continue;
           }
          
           existing_cost = obstacle_heuristic_lookup_table[new_idx];
           if (  existing_cost <= 0.0f ) {
           travel_cost =
           (  (  i <= 3 ) ? 1.0f : sqrt_2 ) * (  1.0f + (  cost_penalty * cost / 252.0f ) );
           new_cost = c_cost + travel_cost;
           if (  existing_cost == 0.0f || -existing_cost > new_cost ) {
           // the negative value means the cell is in the open set
           obstacle_heuristic_lookup_table[new_idx] = -new_cost;
           obstacle_heuristic_queue.emplace_back(  
           new_cost + distanceHeuristic2D(  new_idx,   size_x,   start_x,   start_y ),   new_idx );
           std::push_heap(  
           obstacle_heuristic_queue.begin(   ),   obstacle_heuristic_queue.end(   ),  
           ObstacleHeuristicComparator{} );
           }
           }
           }
           }
          
           if (  idx == start_index ) {
           break;
           }
           }
          
           // return requested_node_cost which has been updated by the search
           // costs are doubled due to downsampling
           return 2.0 * requested_node_cost;
          }
          
     545  float NodeHybrid::getDistanceHeuristic(  
     546   const Coordinates & node_coords,  
     547   const Coordinates & goal_coords,  
           const float & obstacle_heuristic )
          {
           // rotate and translate node_coords such that goal_coords relative is (  0,  0,  0 )
           // Due to the rounding involved in exact cell increments for caching,  
           // this is not an exact replica of a live heuristic,   but has bounded error.
           // (  Usually less than 1 cell )
          
           // This angle is negative since we are de-rotating the current node
           // by the goal angle; cos(  -th ) = cos(  th ) & sin(  -th ) = -sin(  th )
           const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
           const float cos_th = trig_vals.first;
           const float sin_th = -trig_vals.second;
           const float dx = node_coords.x - goal_coords.x;
           const float dy = node_coords.y - goal_coords.y;
          
           double dtheta_bin = node_coords.theta - goal_coords.theta;
           if (  dtheta_bin < 0 ) {
           dtheta_bin += motion_table.num_angle_quantization;
           }
           if (  dtheta_bin > motion_table.num_angle_quantization ) {
           dtheta_bin -= motion_table.num_angle_quantization;
           }
          
           Coordinates node_coords_relative(  
           round(  dx * cos_th - dy * sin_th ),  
           round(  dx * sin_th + dy * cos_th ),  
           round(  dtheta_bin ) );
          
           // Check if the relative node coordinate is within the localized window around the goal
           // to apply the distance heuristic. Since the lookup table is contains only the positive
           // X axis,   we mirror the Y and theta values across the X axis to find the heuristic values.
           float motion_heuristic = 0.0;
           const int floored_size = floor(  size_lookup / 2.0 );
           const int ceiling_size = ceil(  size_lookup / 2.0 );
           const float mirrored_relative_y = abs(  node_coords_relative.y );
           if (  abs(  node_coords_relative.x ) < floored_size && mirrored_relative_y < floored_size ) {
           // Need to mirror angle if Y coordinate was mirrored
           int theta_pos;
           if (  node_coords_relative.y < 0.0 ) {
           theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
           } else {
           theta_pos = node_coords_relative.theta;
           }
           const int x_pos = node_coords_relative.x + floored_size;
           const int y_pos = static_cast<int>(  mirrored_relative_y );
           const int index =
           x_pos * ceiling_size * motion_table.num_angle_quantization +
           y_pos * motion_table.num_angle_quantization +
           theta_pos;
           motion_heuristic = dist_heuristic_lookup_table[index];
           } else if (  obstacle_heuristic == 0.0 ) {
           // If no obstacle heuristic value,   must have some H to use
           // In nominal situations,   this should never be called.
           static ompl::base::ScopedState<> from(  motion_table.state_space ),   to(  motion_table.state_space );
           to[0] = goal_coords.x;
           to[1] = goal_coords.y;
           to[2] = goal_coords.theta * motion_table.num_angle_quantization;
           from[0] = node_coords.x;
           from[1] = node_coords.y;
           from[2] = node_coords.theta * motion_table.num_angle_quantization;
           motion_heuristic = motion_table.state_space->distance(  from(   ),   to(   ) );
           }
          
           return motion_heuristic;
          }
          
     614  void NodeHybrid::precomputeDistanceHeuristic(  
           const float & lookup_table_dim,  
     616   const MotionModel & motion_model,  
           const unsigned int & dim_3_size,  
           const SearchInfo & search_info )
          {
           // Dubin or Reeds-Shepp shortest distances
           if (  motion_model == MotionModel::DUBIN ) {
           motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(  
           search_info.minimum_turning_radius );
           } else if (  motion_model == MotionModel::REEDS_SHEPP ) {
           motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(  
           search_info.minimum_turning_radius );
           } else {
           throw std::runtime_error(  
           "Node attempted to precompute distance heuristics "
           "with invalid motion model!" );
           }
          
           ompl::base::ScopedState<> from(  motion_table.state_space ),   to(  motion_table.state_space );
           to[0] = 0.0;
           to[1] = 0.0;
           to[2] = 0.0;
           size_lookup = lookup_table_dim;
           float motion_heuristic = 0.0;
           unsigned int index = 0;
           int dim_3_size_int = static_cast<int>(  dim_3_size );
           float angular_bin_size = 2 * M_PI / static_cast<float>(  dim_3_size );
          
           // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
           // to help drive the search towards admissible approaches. Deu to symmetries in the
           // Heuristic space,   we need to only store 2 of the 4 quadrants and simply mirror
           // around the X axis any relative node lookup. This reduces memory overhead and increases
           // the size of a window a platform can store in memory.
           dist_heuristic_lookup_table.resize(  size_lookup * ceil(  size_lookup / 2.0 ) * dim_3_size_int );
           for (  float x = ceil(  -size_lookup / 2.0 ); x <= floor(  size_lookup / 2.0 ); x += 1.0 ) {
           for (  float y = 0.0; y <= floor(  size_lookup / 2.0 ); y += 1.0 ) {
           for (  int heading = 0; heading != dim_3_size_int; heading++ ) {
           from[0] = x;
           from[1] = y;
           from[2] = heading * angular_bin_size;
           motion_heuristic = motion_table.state_space->distance(  from(   ),   to(   ) );
           dist_heuristic_lookup_table[index] = motion_heuristic;
           index++;
           }
           }
           }
          }
          
     663  void NodeHybrid::getNeighbors(  
     664   std::function<bool(  const unsigned int &,   nav2_smac_planner::NodeHybrid * & )> & NeighborGetter,  
     665   GridCollisionChecker * collision_checker,  
     666   const bool & traverse_unknown,  
     667   NodeVector & neighbors )
          {
           unsigned int index = 0;
           NodePtr neighbor = nullptr;
           Coordinates initial_node_coords;
           const MotionPoses motion_projections = motion_table.getProjections(  this );
          
           for (  unsigned int i = 0; i != motion_projections.size(   ); i++ ) {
           index = NodeHybrid::getIndex(  
           static_cast<unsigned int>(  motion_projections[i]._x ),  
           static_cast<unsigned int>(  motion_projections[i]._y ),  
           static_cast<unsigned int>(  motion_projections[i]._theta ),  
           motion_table.size_x,   motion_table.num_angle_quantization );
          
           if (  NeighborGetter(  index,   neighbor ) && !neighbor->wasVisited(   ) ) {
           // Cache the initial pose in case it was visited but valid
           // don't want to disrupt continuous coordinate expansion
           initial_node_coords = neighbor->pose;
           neighbor->setPose(  
           Coordinates(  
           motion_projections[i]._x,  
           motion_projections[i]._y,  
           motion_projections[i]._theta ) );
           if (  neighbor->isNodeValid(  traverse_unknown,   collision_checker ) ) {
           neighbor->setMotionPrimitiveIndex(  i );
           neighbors.push_back(  neighbor );
           } else {
           neighbor->setPose(  initial_node_coords );
           }
           }
           }
          }
          
     700  bool NodeHybrid::backtracePath(  CoordinateVector & path )
          {
           if (  !this->parent ) {
           return false;
           }
          
           NodePtr current_node = this;
          
           while (  current_node->parent ) {
           path.push_back(  current_node->pose );
           // Convert angle to radians
           path.back(   ).theta = NodeHybrid::motion_table.getAngleFromBin(  path.back(   ).theta );
           current_node = current_node->parent;
           }
          
           return path.size(   ) > 0;
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_lattice.cpp

          // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <chrono>
          #include <vector>
          #include <memory>
          #include <algorithm>
          #include <queue>
          #include <limits>
          #include <string>
          #include <fstream>
          #include <cmath>
          
          #include "ompl/base/ScopedState.h"
          #include "ompl/base/spaces/DubinsStateSpace.h"
          #include "ompl/base/spaces/ReedsSheppStateSpace.h"
          
          #include "nav2_smac_planner/node_lattice.hpp"
          
          using namespace std::chrono; // NOLINT
          
          namespace nav2_smac_planner
          {
          
          // defining static member for all instance to share
          LatticeMotionTable NodeLattice::motion_table;
          float NodeLattice::size_lookup = 25;
          LookupTable NodeLattice::dist_heuristic_lookup_table;
          
          // Each of these tables are the projected motion models through
          // time and space applied to the search on the current node in
          // continuous map-coordinates (  e.g. not meters but partial map cells )
          // Currently,   these are set to project *at minimum* into a neighboring
          // cell. Though this could be later modified to project a certain
          // amount of time or particular distance forward.
          void LatticeMotionTable::initMotionModel(  
           unsigned int & size_x_in,  
           SearchInfo & search_info )
          {
           size_x = size_x_in;
          
           if (  current_lattice_filepath == search_info.lattice_filepath ) {
           return;
           }
          
           size_x = size_x_in;
           change_penalty = search_info.change_penalty;
           non_straight_penalty = search_info.non_straight_penalty;
           cost_penalty = search_info.cost_penalty;
           reverse_penalty = search_info.reverse_penalty;
           travel_distance_reward = 1.0f - search_info.retrospective_penalty;
           current_lattice_filepath = search_info.lattice_filepath;
           allow_reverse_expansion = search_info.allow_reverse_expansion;
           rotation_penalty = search_info.rotation_penalty;
          
           // Get the metadata about this minimum control set
           lattice_metadata = getLatticeMetadata(  current_lattice_filepath );
           std::ifstream latticeFile(  current_lattice_filepath );
           if (  !latticeFile.is_open(   ) ) {
           throw std::runtime_error(  "Could not open lattice file" );
           }
           nlohmann::json json;
           latticeFile >> json;
           num_angle_quantization = lattice_metadata.number_of_headings;
          
           if (  !state_space ) {
           if (  !allow_reverse_expansion ) {
           state_space = std::make_unique<ompl::base::DubinsStateSpace>(  
           lattice_metadata.min_turning_radius );
           } else {
           state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(  
           lattice_metadata.min_turning_radius );
           }
           }
          
           // Populate the motion primitives at each heading angle
           float prev_start_angle = 0.0;
           std::vector<MotionPrimitive> primitives;
           nlohmann::json json_primitives = json["primitives"];
           for (  unsigned int i = 0; i < json_primitives.size(   ); ++i ) {
           MotionPrimitive new_primitive;
           fromJsonToMotionPrimitive(  json_primitives[i],   new_primitive );
          
           if (  prev_start_angle != new_primitive.start_angle ) {
           motion_primitives.push_back(  primitives );
           primitives.clear(   );
           prev_start_angle = new_primitive.start_angle;
           }
           primitives.push_back(  new_primitive );
           }
           motion_primitives.push_back(  primitives );
          
           // Populate useful precomputed values to be leveraged
           trig_values.reserve(  lattice_metadata.number_of_headings );
           for (  unsigned int i = 0; i < lattice_metadata.heading_angles.size(   ); ++i ) {
           trig_values.emplace_back(  
           cos(  lattice_metadata.heading_angles[i] ),  
           sin(  lattice_metadata.heading_angles[i] ) );
           }
          }
          
          MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(  const NodeLattice * node )
          {
           MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta];
           MotionPrimitivePtrs primitive_projection_list;
           for (  unsigned int i = 0; i != prims_at_heading.size(   ); i++ ) {
           primitive_projection_list.push_back(  &prims_at_heading[i] );
           }
          
           if (  allow_reverse_expansion ) {
           // Find normalized heading bin of the reverse expansion
           double reserve_heading = node->pose.theta - (  num_angle_quantization / 2 );
           if (  reserve_heading < 0 ) {
           reserve_heading += num_angle_quantization;
           }
           if (  reserve_heading > num_angle_quantization ) {
           reserve_heading -= num_angle_quantization;
           }
          
           MotionPrimitives & prims_at_reverse_heading = motion_primitives[reserve_heading];
           for (  unsigned int i = 0; i != prims_at_reverse_heading.size(   ); i++ ) {
           primitive_projection_list.push_back(  &prims_at_reverse_heading[i] );
           }
           }
          
           return primitive_projection_list;
          }
          
          LatticeMetadata LatticeMotionTable::getLatticeMetadata(  const std::string & lattice_filepath )
          {
           std::ifstream lattice_file(  lattice_filepath );
           if (  !lattice_file.is_open(   ) ) {
           throw std::runtime_error(  "Could not open lattice file!" );
           }
          
           nlohmann::json j;
           lattice_file >> j;
           LatticeMetadata metadata;
           fromJsonToMetaData(  j["lattice_metadata"],   metadata );
           return metadata;
          }
          
          unsigned int LatticeMotionTable::getClosestAngularBin(  const double & theta )
          {
           float min_dist = std::numeric_limits<float>::max(   );
           unsigned int closest_idx = 0;
           float dist = 0.0;
           for (  unsigned int i = 0; i != lattice_metadata.heading_angles.size(   ); i++ ) {
           dist = fabs(  angles::shortest_angular_distance(  theta,   lattice_metadata.heading_angles[i] ) );
           if (  dist < min_dist ) {
           min_dist = dist;
           closest_idx = i;
           }
           }
           return closest_idx;
          }
          
          float & LatticeMotionTable::getAngleFromBin(  const unsigned int & bin_idx )
          {
           return lattice_metadata.heading_angles[bin_idx];
          }
          
     175  NodeLattice::NodeLattice(  const unsigned int index )
          : parent(  nullptr ),  
     177   pose(  0.0f,   0.0f,   0.0f ),  
           _cell_cost(  std::numeric_limits<float>::quiet_NaN(   ) ),  
           _accumulated_cost(  std::numeric_limits<float>::max(   ) ),  
           _index(  index ),  
           _was_visited(  false ),  
           _motion_primitive(  nullptr ),  
           _backwards(  false )
          {
          }
          
     187  NodeLattice::~NodeLattice(   )
          {
           parent = nullptr;
          }
          
     192  void NodeLattice::reset(   )
          {
           parent = nullptr;
           _cell_cost = std::numeric_limits<float>::quiet_NaN(   );
           _accumulated_cost = std::numeric_limits<float>::max(   );
           _was_visited = false;
           pose.x = 0.0f;
           pose.y = 0.0f;
           pose.theta = 0.0f;
           _motion_primitive = nullptr;
           _backwards = false;
          }
          
     205  bool NodeLattice::isNodeValid(  
     206   const bool & traverse_unknown,  
     207   GridCollisionChecker * collision_checker,  
           MotionPrimitive * motion_primitive,  
     209   bool is_backwards )
          {
           // Check primitive end pose
           // Convert grid quantization of primitives to radians,   then collision checker quantization
           static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles(   ).size(   );
           const double & angle = motion_table.getAngleFromBin(  this->pose.theta ) / bin_size;
           if (  collision_checker->inCollision(  
           this->pose.x,   this->pose.y,   angle /*bin in collision checker*/,   traverse_unknown ) )
           {
           return false;
           }
          
           // Set the cost of a node to the highest cost across the primitive
           float max_cell_cost = collision_checker->getCost(   );
          
           // If valid motion primitives are set,   check intermediary poses > 1 cell apart
           if (  motion_primitive ) {
           const float pi_2 = 2.0 * M_PI;
           const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
           const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution;
           MotionPose last_pose(  1e9,   1e9,   1e9 ),   pose_dist(  0.0,   0.0,   0.0 );
          
           // Back out the initial node starting point to move motion primitive relative to
           MotionPose initial_pose,   prim_pose;
           initial_pose._x = this->pose.x - (  motion_primitive->poses.back(   )._x / grid_resolution );
           initial_pose._y = this->pose.y - (  motion_primitive->poses.back(   )._y / grid_resolution );
           initial_pose._theta = motion_table.getAngleFromBin(  motion_primitive->start_angle );
          
           for (  auto it = motion_primitive->poses.begin(   ); it != motion_primitive->poses.end(   ); ++it ) {
           // poses are in metric coordinates from (  0,   0 ),   not grid space yet
           pose_dist = *it - last_pose;
           // Avoid square roots by (  hypot(  x,   y ) > res ) == (  x*x+y*y > diag*diag )
           if (  pose_dist._x * pose_dist._x + pose_dist._y * pose_dist._y > resolution_diag_sq ) {
           last_pose = *it;
           // Convert primitive pose into grid space if it should be checked
           prim_pose._x = initial_pose._x + (  it->_x / grid_resolution );
           prim_pose._y = initial_pose._y + (  it->_y / grid_resolution );
           // If reversing,   invert the angle because the robot is backing into the primitive
           // not driving forward with it
           if (  is_backwards ) {
           prim_pose._theta = std::fmod(  it->_theta + M_PI,   pi_2 );
           } else {
           prim_pose._theta = it->_theta;
           }
           if (  collision_checker->inCollision(  
           prim_pose._x,  
           prim_pose._y,  
           prim_pose._theta / bin_size /*bin in collision checker*/,  
           traverse_unknown ) )
           {
           return false;
           }
           max_cell_cost = std::max(  max_cell_cost,   collision_checker->getCost(   ) );
           }
           }
           }
          
           _cell_cost = max_cell_cost;
           return true;
          }
          
     270  float NodeLattice::getTraversalCost(  const NodePtr & child )
          {
           const float normalized_cost = child->getCost(   ) / 252.0;
           if (  std::isnan(  normalized_cost ) ) {
           throw std::runtime_error(  
           "Node attempted to get traversal "
           "cost without a known collision cost!" );
           }
          
           // this is the first node
           MotionPrimitive * prim = this->getMotionPrimitive(   );
           MotionPrimitive * transition_prim = child->getMotionPrimitive(   );
           const float prim_length =
           transition_prim->trajectory_length / motion_table.lattice_metadata.grid_resolution;
           if (  prim == nullptr ) {
           return prim_length;
           }
          
           // Pure rotation in place 1 angular bin in either direction
           if (  transition_prim->trajectory_length < 1e-4 ) {
           return motion_table.rotation_penalty * (  1.0 + motion_table.cost_penalty * normalized_cost );
           }
          
           float travel_cost = 0.0;
           float travel_cost_raw = prim_length *
           (  motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost );
          
           if (  transition_prim->arc_length < 0.001 ) {
           // New motion is a straight motion,   no additional costs to be applied
           travel_cost = travel_cost_raw;
           } else {
           if (  prim->left_turn == transition_prim->left_turn ) {
           // Turning motion but keeps in same general direction: encourages to commit to actions
           travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
           } else {
           // Turning motion and velocity directions: penalizes wiggling.
           travel_cost = travel_cost_raw *
           (  motion_table.non_straight_penalty + motion_table.change_penalty );
           }
           }
          
           // If backwards flag is set,   this primitive is moving in reverse
           if (  child->isBackward(   ) ) {
           // reverse direction
           travel_cost *= motion_table.reverse_penalty;
           }
          
           return travel_cost;
          }
          
     320  float NodeLattice::getHeuristicCost(  
     321   const Coordinates & node_coords,  
     322   const Coordinates & goal_coords,  
     323   const nav2_costmap_2d::Costmap2D * costmap )
          {
           // get obstacle heuristic value
           const float obstacle_heuristic = getObstacleHeuristic(  
           node_coords,   goal_coords,   motion_table.cost_penalty );
           const float distance_heuristic =
           getDistanceHeuristic(  node_coords,   goal_coords,   obstacle_heuristic );
           return std::max(  obstacle_heuristic,   distance_heuristic );
          }
          
     333  void NodeLattice::initMotionModel(  
     334   const MotionModel & motion_model,  
           unsigned int & size_x,  
           unsigned int & /*size_y*/,  
           unsigned int & /*num_angle_quantization*/,  
           SearchInfo & search_info )
          {
           if (  motion_model != MotionModel::STATE_LATTICE ) {
           throw std::runtime_error(  
           "Invalid motion model for Lattice node. Please select"
           " STATE_LATTICE and provide a valid lattice file." );
           }
          
           motion_table.initMotionModel(  size_x,   search_info );
          }
          
     349  float NodeLattice::getDistanceHeuristic(  
     350   const Coordinates & node_coords,  
     351   const Coordinates & goal_coords,  
           const float & obstacle_heuristic )
          {
           // rotate and translate node_coords such that goal_coords relative is (  0,  0,  0 )
           // Due to the rounding involved in exact cell increments for caching,  
           // this is not an exact replica of a live heuristic,   but has bounded error.
           // (  Usually less than 1 cell length )
          
           // This angle is negative since we are de-rotating the current node
           // by the goal angle; cos(  -th ) = cos(  th ) & sin(  -th ) = -sin(  th )
           const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
           const float cos_th = trig_vals.first;
           const float sin_th = -trig_vals.second;
           const float dx = node_coords.x - goal_coords.x;
           const float dy = node_coords.y - goal_coords.y;
          
           double dtheta_bin = node_coords.theta - goal_coords.theta;
           if (  dtheta_bin < 0 ) {
           dtheta_bin += motion_table.num_angle_quantization;
           }
           if (  dtheta_bin > motion_table.num_angle_quantization ) {
           dtheta_bin -= motion_table.num_angle_quantization;
           }
          
           Coordinates node_coords_relative(  
           round(  dx * cos_th - dy * sin_th ),  
           round(  dx * sin_th + dy * cos_th ),  
           round(  dtheta_bin ) );
          
           // Check if the relative node coordinate is within the localized window around the goal
           // to apply the distance heuristic. Since the lookup table is contains only the positive
           // X axis,   we mirror the Y and theta values across the X axis to find the heuristic values.
           float motion_heuristic = 0.0;
           const int floored_size = floor(  size_lookup / 2.0 );
           const int ceiling_size = ceil(  size_lookup / 2.0 );
           const float mirrored_relative_y = abs(  node_coords_relative.y );
           if (  abs(  node_coords_relative.x ) < floored_size && mirrored_relative_y < floored_size ) {
           // Need to mirror angle if Y coordinate was mirrored
           int theta_pos;
           if (  node_coords_relative.y < 0.0 ) {
           theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
           } else {
           theta_pos = node_coords_relative.theta;
           }
           const int x_pos = node_coords_relative.x + floored_size;
           const int y_pos = static_cast<int>(  mirrored_relative_y );
           const int index =
           x_pos * ceiling_size * motion_table.num_angle_quantization +
           y_pos * motion_table.num_angle_quantization +
           theta_pos;
           motion_heuristic = dist_heuristic_lookup_table[index];
           } else if (  obstacle_heuristic == 0.0 ) {
           static ompl::base::ScopedState<> from(  motion_table.state_space ),   to(  motion_table.state_space );
           to[0] = goal_coords.x;
           to[1] = goal_coords.y;
           to[2] = motion_table.getAngleFromBin(  goal_coords.theta );
           from[0] = node_coords.x;
           from[1] = node_coords.y;
           from[2] = motion_table.getAngleFromBin(  node_coords.theta );
           motion_heuristic = motion_table.state_space->distance(  from(   ),   to(   ) );
           }
          
           return motion_heuristic;
          }
          
     416  void NodeLattice::precomputeDistanceHeuristic(  
           const float & lookup_table_dim,  
     418   const MotionModel & motion_model,  
           const unsigned int & dim_3_size,  
           const SearchInfo & search_info )
          {
           // Dubin or Reeds-Shepp shortest distances
           if (  !search_info.allow_reverse_expansion ) {
           motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(  
           search_info.minimum_turning_radius );
           } else {
           motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(  
           search_info.minimum_turning_radius );
           }
           motion_table.lattice_metadata =
           LatticeMotionTable::getLatticeMetadata(  search_info.lattice_filepath );
          
           ompl::base::ScopedState<> from(  motion_table.state_space ),   to(  motion_table.state_space );
           to[0] = 0.0;
           to[1] = 0.0;
           to[2] = 0.0;
           size_lookup = lookup_table_dim;
           float motion_heuristic = 0.0;
           unsigned int index = 0;
           int dim_3_size_int = static_cast<int>(  dim_3_size );
          
           // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
           // to help drive the search towards admissible approaches. Deu to symmetries in the
           // Heuristic space,   we need to only store 2 of the 4 quadrants and simply mirror
           // around the X axis any relative node lookup. This reduces memory overhead and increases
           // the size of a window a platform can store in memory.
           dist_heuristic_lookup_table.resize(  size_lookup * ceil(  size_lookup / 2.0 ) * dim_3_size_int );
           for (  float x = ceil(  -size_lookup / 2.0 ); x <= floor(  size_lookup / 2.0 ); x += 1.0 ) {
           for (  float y = 0.0; y <= floor(  size_lookup / 2.0 ); y += 1.0 ) {
           for (  int heading = 0; heading != dim_3_size_int; heading++ ) {
           from[0] = x;
           from[1] = y;
           from[2] = motion_table.getAngleFromBin(  heading );
           motion_heuristic = motion_table.state_space->distance(  from(   ),   to(   ) );
           dist_heuristic_lookup_table[index] = motion_heuristic;
           index++;
           }
           }
           }
          }
          
     462  void NodeLattice::getNeighbors(  
     463   std::function<bool(  const unsigned int &,   nav2_smac_planner::NodeLattice * & )> & NeighborGetter,  
     464   GridCollisionChecker * collision_checker,  
     465   const bool & traverse_unknown,  
     466   NodeVector & neighbors )
          {
           unsigned int index = 0;
           float angle;
           bool backwards = false;
           NodePtr neighbor = nullptr;
           Coordinates initial_node_coords,   motion_projection;
           MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(  this );
           const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
          
           unsigned int direction_change_idx = 1e9;
           for (  unsigned int i = 0; i != motion_primitives.size(   ); i++ ) {
           if (  motion_primitives[0]->start_angle != motion_primitives[i]->start_angle ) {
           direction_change_idx = i;
           break;
           }
           }
          
           for (  unsigned int i = 0; i != motion_primitives.size(   ); i++ ) {
           const MotionPose & end_pose = motion_primitives[i]->poses.back(   );
           motion_projection.x = this->pose.x + (  end_pose._x / grid_resolution );
           motion_projection.y = this->pose.y + (  end_pose._y / grid_resolution );
           motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/;
          
           index = NodeLattice::getIndex(  
           static_cast<unsigned int>(  motion_projection.x ),  
           static_cast<unsigned int>(  motion_projection.y ),  
           static_cast<unsigned int>(  motion_projection.theta ) );
          
           if (  NeighborGetter(  index,   neighbor ) && !neighbor->wasVisited(   ) ) {
           // Cache the initial pose in case it was visited but valid
           // don't want to disrupt continuous coordinate expansion
           initial_node_coords = neighbor->pose;
           // if i >= idx,   then we're in a reversing primitive. In that situation,  
           // the orientation of the robot is mirrored from what it would otherwise
           // appear to be from the motion primitives file. We want to take this into
           // account in case the robot base footprint is asymmetric.
           backwards = false;
           angle = motion_projection.theta;
           if (  i >= direction_change_idx ) {
           backwards = true;
           angle = motion_projection.theta - (  motion_table.num_angle_quantization / 2 );
           if (  angle < 0 ) {
           angle += motion_table.num_angle_quantization;
           }
           if (  angle > motion_table.num_angle_quantization ) {
           angle -= motion_table.num_angle_quantization;
           }
           }
          
           neighbor->setPose(  
           Coordinates(  
           motion_projection.x,  
           motion_projection.y,  
           angle ) );
          
           // Using a special isNodeValid API here,   giving the motion primitive to use to
           // validity check the transition of the current node to the new node over
           if (  neighbor->isNodeValid(  
           traverse_unknown,   collision_checker,   motion_primitives[i],   backwards ) )
           {
           neighbor->setMotionPrimitive(  motion_primitives[i] );
           // Marking if this search was obtained in the reverse direction
           neighbor->backwards(  backwards );
           neighbors.push_back(  neighbor );
           } else {
           neighbor->setPose(  initial_node_coords );
           }
           }
           }
          }
          
     538  bool NodeLattice::backtracePath(  CoordinateVector & path )
          {
           if (  !this->parent ) {
           return false;
           }
          
           Coordinates initial_pose,   prim_pose;
           NodePtr current_node = this;
           MotionPrimitive * prim = nullptr;
           const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution;
           const float pi_2 = 2.0 * M_PI;
          
           while (  current_node->parent ) {
           prim = current_node->getMotionPrimitive(   );
           // if motion primitive is valid,   then was searched (  rather than analytically expanded ),  
           // include dense path of subpoints making up the primitive at grid resolution
           if (  prim ) {
           initial_pose.x = current_node->pose.x - (  prim->poses.back(   )._x / grid_resolution );
           initial_pose.y = current_node->pose.y - (  prim->poses.back(   )._y / grid_resolution );
           initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(  prim->start_angle );
          
           for (  auto it = prim->poses.crbegin(   ); it != prim->poses.crend(   ); ++it ) {
           // Convert primitive pose into grid space if it should be checked
           prim_pose.x = initial_pose.x + (  it->_x / grid_resolution );
           prim_pose.y = initial_pose.y + (  it->_y / grid_resolution );
           // If reversing,   invert the angle because the robot is backing into the primitive
           // not driving forward with it
           if (  current_node->isBackward(   ) ) {
           prim_pose.theta = std::fmod(  it->_theta + M_PI,   pi_2 );
           } else {
           prim_pose.theta = it->_theta;
           }
           path.push_back(  prim_pose );
           }
           } else {
           // For analytic expansion nodes where there is no valid motion primitive
           path.push_back(  current_node->pose );
           path.back(   ).theta = NodeLattice::motion_table.getAngleFromBin(  path.back(   ).theta );
           }
          
           current_node = current_node->parent;
           }
          
           return path.size(   ) > 0;
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/smac_planner_2d.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <vector>
          #include <limits>
          #include <algorithm>
          
          #include "nav2_smac_planner/smac_planner_2d.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          // #define BENCHMARK_TESTING
          
          namespace nav2_smac_planner
          {
          using namespace std::chrono; // NOLINT
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
      32  SmacPlanner2D::SmacPlanner2D(   )
          : _a_star(  nullptr ),  
           _collision_checker(  nullptr,   1 ),  
           _smoother(  nullptr ),  
           _costmap(  nullptr ),  
           _costmap_downsampler(  nullptr )
          {
          }
          
      41  SmacPlanner2D::~SmacPlanner2D(   )
          {
           RCLCPP_INFO(  
           _logger,   "Destroying plugin %s of type SmacPlanner2D",  
           _name.c_str(   ) );
          }
          
      48  void SmacPlanner2D::configure(  
      49   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      50   std::string name,   std::shared_ptr<tf2_ros::Buffer>/*tf*/,  
      51   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           _node = parent;
           auto node = parent.lock(   );
           _logger = node->get_logger(   );
           _clock = node->get_clock(   );
           _costmap = costmap_ros->getCostmap(   );
           _name = name;
           _global_frame = costmap_ros->getGlobalFrameID(   );
          
           RCLCPP_INFO(  _logger,   "Configuring %s of type SmacPlanner2D",   name.c_str(   ) );
          
           // General planner params
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".tolerance",   rclcpp::ParameterValue(  0.125 ) );
           _tolerance = static_cast<float>(  node->get_parameter(  name + ".tolerance" ).as_double(   ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".downsample_costmap",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".downsample_costmap",   _downsample_costmap );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".downsampling_factor",   rclcpp::ParameterValue(  1 ) );
           node->get_parameter(  name + ".downsampling_factor",   _downsampling_factor );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".cost_travel_multiplier",   rclcpp::ParameterValue(  1.0 ) );
           node->get_parameter(  name + ".cost_travel_multiplier",   _search_info.cost_penalty );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".allow_unknown",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".allow_unknown",   _allow_unknown );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_iterations",   rclcpp::ParameterValue(  1000000 ) );
           node->get_parameter(  name + ".max_iterations",   _max_iterations );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_on_approach_iterations",   rclcpp::ParameterValue(  1000 ) );
           node->get_parameter(  name + ".max_on_approach_iterations",   _max_on_approach_iterations );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".use_final_approach_orientation",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".use_final_approach_orientation",   _use_final_approach_orientation );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_planning_time",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name + ".max_planning_time",   _max_planning_time );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".motion_model_for_search",   rclcpp::ParameterValue(  std::string(  "MOORE" ) ) );
           node->get_parameter(  name + ".motion_model_for_search",   _motion_model_for_search );
           _motion_model = fromString(  _motion_model_for_search );
           if (  _motion_model == MotionModel::UNKNOWN ) {
           RCLCPP_WARN(  
           _logger,  
           "Unable to get MotionModel search type. Given '%s',   "
           "valid options are MOORE,   VON_NEUMANN,   DUBIN,   REEDS_SHEPP.",  
           _motion_model_for_search.c_str(   ) );
           }
          
           if (  _max_on_approach_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "On approach iteration selected as <= 0,   "
           "disabling tolerance and on approach iterations." );
           _max_on_approach_iterations = std::numeric_limits<int>::max(   );
           }
          
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
          
           // Initialize collision checker
           _collision_checker = GridCollisionChecker(  _costmap,   1 /*for 2D,   most be 1*/ );
           _collision_checker.setFootprint(  
           costmap_ros->getRobotFootprint(   ),  
           true /*for 2D,   most use radius*/,  
           0.0 /*for 2D cost at inscribed isn't relevent*/ );
          
           // Initialize A* template
           _a_star = std::make_unique<AStarAlgorithm<Node2D>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           _max_on_approach_iterations,  
           _max_planning_time,  
           0.0 /*unused for 2D*/,  
           1.0 /*unused for 2D*/ );
          
           // Initialize path smoother
           SmootherParams params;
           params.get(  node,   name );
           params.holonomic_ = true; // So smoother will treat this as a grid search
           _smoother = std::make_unique<Smoother>(  params );
           _smoother->initialize(  1e-50 /*No valid minimum turning radius for 2D*/ );
          
           // Initialize costmap downsampler
           if (  _downsample_costmap && _downsampling_factor > 1 ) {
           std::string topic_name = "downsampled_costmap";
           _costmap_downsampler = std::make_unique<CostmapDownsampler>(   );
           _costmap_downsampler->on_configure(  
           node,   _global_frame,   topic_name,   _costmap,   _downsampling_factor );
           }
          
           _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>(  "unsmoothed_plan",   1 );
          
           RCLCPP_INFO(  
           _logger,   "Configured plugin %s of type SmacPlanner2D with "
           "tolerance %.2f,   maximum iterations %i,   "
           "max on approach iterations %i,   and %s. Using motion model: %s.",  
           _name.c_str(   ),   _tolerance,   _max_iterations,   _max_on_approach_iterations,  
           _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",  
           toString(  _motion_model ).c_str(   ) );
          }
          
     163  void SmacPlanner2D::activate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Activating plugin %s of type SmacPlanner2D",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_activate(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_activate(   );
           }
           auto node = _node.lock(   );
           // Add callback for dynamic parameters
           _dyn_params_handler = node->add_on_set_parameters_callback(  
           std::bind(  &SmacPlanner2D::dynamicParametersCallback,   this,   _1 ) );
          }
          
     178  void SmacPlanner2D::deactivate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Deactivating plugin %s of type SmacPlanner2D",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_deactivate(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_deactivate(   );
           }
           _dyn_params_handler.reset(   );
          }
          
     190  void SmacPlanner2D::cleanup(   )
          {
           RCLCPP_INFO(  
           _logger,   "Cleaning up plugin %s of type SmacPlanner2D",  
           _name.c_str(   ) );
           _a_star.reset(   );
           _smoother.reset(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_cleanup(   );
           _costmap_downsampler.reset(   );
           }
           _raw_plan_publisher.reset(   );
          }
          
     204  nav_msgs::msg::Path SmacPlanner2D::createPlan(  
     205   const geometry_msgs::msg::PoseStamped & start,  
     206   const geometry_msgs::msg::PoseStamped & goal )
          {
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
           steady_clock::time_point a = steady_clock::now(   );
          
           std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(  *(  _costmap->getMutex(   ) ) );
          
           // Downsample costmap,   if required
           nav2_costmap_2d::Costmap2D * costmap = _costmap;
           if (  _costmap_downsampler ) {
           costmap = _costmap_downsampler->downsample(  _downsampling_factor );
           _collision_checker.setCostmap(  costmap );
           }
          
           // Set collision checker and costmap information
           _a_star->setCollisionChecker(  &_collision_checker );
          
           // Set starting point
           unsigned int mx_start,   my_start,   mx_goal,   my_goal;
           costmap->worldToMap(  start.pose.position.x,   start.pose.position.y,   mx_start,   my_start );
           _a_star->setStart(  mx_start,   my_start,   0 );
          
           // Set goal point
           costmap->worldToMap(  goal.pose.position.x,   goal.pose.position.y,   mx_goal,   my_goal );
           _a_star->setGoal(  mx_goal,   my_goal,   0 );
          
           // Setup message
           nav_msgs::msg::Path plan;
           plan.header.stamp = _clock->now(   );
           plan.header.frame_id = _global_frame;
           geometry_msgs::msg::PoseStamped pose;
           pose.header = plan.header;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
          
           // Corner case of start and goal beeing on the same cell
           if (  mx_start == mx_goal && my_start == my_goal ) {
           if (  costmap->getCost(  mx_start,   my_start ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           RCLCPP_WARN(  _logger,   "Failed to create a unique pose path because of obstacles" );
           return plan;
           }
           pose.pose = start.pose;
           // if we have a different start and goal orientation,   set the unique path pose to the goal
           // orientation,   unless use_final_approach_orientation=true where we need it to be the start
           // orientation to avoid movement from the local planner
           if (  start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation ) {
           pose.pose.orientation = goal.pose.orientation;
           }
           plan.poses.push_back(  pose );
           return plan;
           }
          
           // Compute plan
           Node2D::CoordinateVector path;
           int num_iterations = 0;
           std::string error;
           try {
           if (  !_a_star->createPath(  
           path,   num_iterations,   _tolerance / static_cast<float>(  costmap->getResolution(   ) ) ) )
           {
           if (  num_iterations < _a_star->getMaxIterations(   ) ) {
           error = std::string(  "no valid path found" );
           } else {
           error = std::string(  "exceeded maximum iterations" );
           }
           }
           } catch (  const std::runtime_error & e ) {
           error = "invalid use: ";
           error += e.what(   );
           }
          
           if (  !error.empty(   ) ) {
           RCLCPP_WARN(  
           _logger,  
           "%s: failed to create plan,   %s.",  
           _name.c_str(   ),   error.c_str(   ) );
           return plan;
           }
          
           // Convert to world coordinates
           plan.poses.reserve(  path.size(   ) );
           for (  int i = path.size(   ) - 1; i >= 0; --i ) {
           pose.pose = getWorldCoords(  path[i].x,   path[i].y,   costmap );
           plan.poses.push_back(  pose );
           }
          
           // Publish raw path for debug
           if (  _raw_plan_publisher->get_subscription_count(   ) > 0 ) {
           _raw_plan_publisher->publish(  plan );
           }
          
           // Find how much time we have left to do smoothing
           steady_clock::time_point b = steady_clock::now(   );
           duration<double> time_span = duration_cast<duration<double>>(  b - a );
           double time_remaining = _max_planning_time - static_cast<double>(  time_span.count(   ) );
          
          #ifdef BENCHMARK_TESTING
           std::cout << "It took " << time_span.count(   ) * 1000 <<
           " milliseconds with " << num_iterations << " iterations." << std::endl;
          #endif
          
           // Smooth plan
           _smoother->smooth(  plan,   costmap,   time_remaining );
          
           // If use_final_approach_orientation=true,   interpolate the last pose orientation from the
           // previous pose to set the orientation to the 'final approach' orientation of the robot so
           // it does not rotate.
           // And deal with corner case of plan of length 1
           // If use_final_approach_orientation=false (  default ),   override last pose orientation to match goal
           size_t plan_size = plan.poses.size(   );
           if (  _use_final_approach_orientation ) {
           if (  plan_size == 1 ) {
           plan.poses.back(   ).pose.orientation = start.pose.orientation;
           } else if (  plan_size > 1 ) {
           double dx,   dy,   theta;
           auto last_pose = plan.poses.back(   ).pose.position;
           auto approach_pose = plan.poses[plan_size - 2].pose.position;
           dx = last_pose.x - approach_pose.x;
           dy = last_pose.y - approach_pose.y;
           theta = atan2(  dy,   dx );
           plan.poses.back(   ).pose.orientation =
           nav2_util::geometry_utils::orientationAroundZAxis(  theta );
           }
           } else if (  plan_size > 0 ) {
           plan.poses.back(   ).pose.orientation = goal.pose.orientation;
           }
          
           return plan;
          }
          
          rcl_interfaces::msg::SetParametersResult
     340  SmacPlanner2D::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
          
           bool reinit_a_star = false;
           bool reinit_downsampler = false;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == _name + ".tolerance" ) {
           _tolerance = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".cost_travel_multiplier" ) {
           reinit_a_star = true;
           _search_info.cost_penalty = parameter.as_double(   );
           } else if (  name == _name + ".max_planning_time" ) {
           reinit_a_star = true;
           _max_planning_time = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == _name + ".downsample_costmap" ) {
           reinit_downsampler = true;
           _downsample_costmap = parameter.as_bool(   );
           } else if (  name == _name + ".allow_unknown" ) {
           reinit_a_star = true;
           _allow_unknown = parameter.as_bool(   );
           } else if (  name == _name + ".use_final_approach_orientation" ) {
           _use_final_approach_orientation = parameter.as_bool(   );
           }
           } else if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == _name + ".downsampling_factor" ) {
           reinit_downsampler = true;
           _downsampling_factor = parameter.as_int(   );
           } else if (  name == _name + ".max_iterations" ) {
           reinit_a_star = true;
           _max_iterations = parameter.as_int(   );
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
           } else if (  name == _name + ".max_on_approach_iterations" ) {
           reinit_a_star = true;
           _max_on_approach_iterations = parameter.as_int(   );
           if (  _max_on_approach_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "On approach iteration selected as <= 0,   "
           "disabling tolerance and on approach iterations." );
           _max_on_approach_iterations = std::numeric_limits<int>::max(   );
           }
           }
           } else if (  type == ParameterType::PARAMETER_STRING ) {
           if (  name == _name + ".motion_model_for_search" ) {
           reinit_a_star = true;
           _motion_model = fromString(  parameter.as_string(   ) );
           if (  _motion_model == MotionModel::UNKNOWN ) {
           RCLCPP_WARN(  
           _logger,  
           "Unable to get MotionModel search type. Given '%s',   "
           "valid options are MOORE,   VON_NEUMANN,   DUBIN,   REEDS_SHEPP.",  
           _motion_model_for_search.c_str(   ) );
           }
           }
           }
           }
          
           // Re-init if needed with mutex lock (  to avoid re-init while creating a plan )
           if (  reinit_a_star || reinit_downsampler ) {
           // Re-Initialize A* template
           if (  reinit_a_star ) {
           _a_star = std::make_unique<AStarAlgorithm<Node2D>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           _max_on_approach_iterations,  
           _max_planning_time,  
           0.0 /*unused for 2D*/,  
           1.0 /*unused for 2D*/ );
           }
          
           // Re-Initialize costmap downsampler
           if (  reinit_downsampler ) {
           if (  _downsample_costmap && _downsampling_factor > 1 ) {
           auto node = _node.lock(   );
           std::string topic_name = "downsampled_costmap";
           _costmap_downsampler = std::make_unique<CostmapDownsampler>(   );
           _costmap_downsampler->on_configure(  
           node,   _global_frame,   topic_name,   _costmap,   _downsampling_factor );
           }
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_smac_planner
          
          #include "pluginlib/class_list_macros.hpp"
     442  PLUGINLIB_EXPORT_CLASS(  nav2_smac_planner::SmacPlanner2D,   nav2_core::GlobalPlanner )

navigation2/nav2_smac_planner/src/smac_planner_hybrid.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <vector>
          #include <algorithm>
          #include <limits>
          
          #include "Eigen/Core"
          #include "nav2_smac_planner/smac_planner_hybrid.hpp"
          
          // #define BENCHMARK_TESTING
          
          namespace nav2_smac_planner
          {
          
          using namespace std::chrono; // NOLINT
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
      33  SmacPlannerHybrid::SmacPlannerHybrid(   )
          : _a_star(  nullptr ),  
           _collision_checker(  nullptr,   1 ),  
           _smoother(  nullptr ),  
           _costmap(  nullptr ),  
           _costmap_downsampler(  nullptr )
          {
          }
          
      42  SmacPlannerHybrid::~SmacPlannerHybrid(   )
          {
           RCLCPP_INFO(  
           _logger,   "Destroying plugin %s of type SmacPlannerHybrid",  
           _name.c_str(   ) );
          }
          
      49  void SmacPlannerHybrid::configure(  
      50   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      51   std::string name,   std::shared_ptr<tf2_ros::Buffer>/*tf*/,  
      52   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           _node = parent;
           auto node = parent.lock(   );
           _logger = node->get_logger(   );
           _clock = node->get_clock(   );
           _costmap = costmap_ros->getCostmap(   );
           _costmap_ros = costmap_ros;
           _name = name;
           _global_frame = costmap_ros->getGlobalFrameID(   );
          
           RCLCPP_INFO(  _logger,   "Configuring %s of type SmacPlannerHybrid",   name.c_str(   ) );
          
           int angle_quantizations;
           double analytic_expansion_max_length_m;
           bool smooth_path;
          
           // General planner params
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".downsample_costmap",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".downsample_costmap",   _downsample_costmap );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".downsampling_factor",   rclcpp::ParameterValue(  1 ) );
           node->get_parameter(  name + ".downsampling_factor",   _downsampling_factor );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".angle_quantization_bins",   rclcpp::ParameterValue(  72 ) );
           node->get_parameter(  name + ".angle_quantization_bins",   angle_quantizations );
           _angle_bin_size = 2.0 * M_PI / angle_quantizations;
           _angle_quantizations = static_cast<unsigned int>(  angle_quantizations );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".allow_unknown",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".allow_unknown",   _allow_unknown );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_iterations",   rclcpp::ParameterValue(  1000000 ) );
           node->get_parameter(  name + ".max_iterations",   _max_iterations );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".smooth_path",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".smooth_path",   smooth_path );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".minimum_turning_radius",   rclcpp::ParameterValue(  0.4 ) );
           node->get_parameter(  name + ".minimum_turning_radius",   _minimum_turning_radius_global_coords );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".cache_obstacle_heuristic",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".cache_obstacle_heuristic",   _search_info.cache_obstacle_heuristic );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".reverse_penalty",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name + ".reverse_penalty",   _search_info.reverse_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".change_penalty",   rclcpp::ParameterValue(  0.0 ) );
           node->get_parameter(  name + ".change_penalty",   _search_info.change_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".non_straight_penalty",   rclcpp::ParameterValue(  1.2 ) );
           node->get_parameter(  name + ".non_straight_penalty",   _search_info.non_straight_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".cost_penalty",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name + ".cost_penalty",   _search_info.cost_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".retrospective_penalty",   rclcpp::ParameterValue(  0.015 ) );
           node->get_parameter(  name + ".retrospective_penalty",   _search_info.retrospective_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".analytic_expansion_ratio",   rclcpp::ParameterValue(  3.5 ) );
           node->get_parameter(  name + ".analytic_expansion_ratio",   _search_info.analytic_expansion_ratio );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".analytic_expansion_max_length",   rclcpp::ParameterValue(  3.0 ) );
           node->get_parameter(  name + ".analytic_expansion_max_length",   analytic_expansion_max_length_m );
           _search_info.analytic_expansion_max_length =
           analytic_expansion_max_length_m / _costmap->getResolution(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_planning_time",   rclcpp::ParameterValue(  5.0 ) );
           node->get_parameter(  name + ".max_planning_time",   _max_planning_time );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".lookup_table_size",   rclcpp::ParameterValue(  20.0 ) );
           node->get_parameter(  name + ".lookup_table_size",   _lookup_table_size );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".motion_model_for_search",   rclcpp::ParameterValue(  std::string(  "DUBIN" ) ) );
           node->get_parameter(  name + ".motion_model_for_search",   _motion_model_for_search );
           _motion_model = fromString(  _motion_model_for_search );
           if (  _motion_model == MotionModel::UNKNOWN ) {
           RCLCPP_WARN(  
           _logger,  
           "Unable to get MotionModel search type. Given '%s',   "
           "valid options are MOORE,   VON_NEUMANN,   DUBIN,   REEDS_SHEPP,   STATE_LATTICE.",  
           _motion_model_for_search.c_str(   ) );
           }
          
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
          
           // convert to grid coordinates
           if (  !_downsample_costmap ) {
           _downsampling_factor = 1;
           }
           _search_info.minimum_turning_radius =
           _minimum_turning_radius_global_coords / (  _costmap->getResolution(   ) * _downsampling_factor );
           _lookup_table_dim =
           static_cast<float>(  _lookup_table_size ) /
           static_cast<float>(  _costmap->getResolution(   ) * _downsampling_factor );
          
           // Make sure its a whole number
           _lookup_table_dim = static_cast<float>(  static_cast<int>(  _lookup_table_dim ) );
          
           // Make sure its an odd number
           if (  static_cast<int>(  _lookup_table_dim ) % 2 == 0 ) {
           RCLCPP_INFO(  
           _logger,  
           "Even sized heuristic lookup table size set %f,   increasing size by 1 to make odd",  
           _lookup_table_dim );
           _lookup_table_dim += 1.0;
           }
          
           // Initialize collision checker
           _collision_checker = GridCollisionChecker(  _costmap,   _angle_quantizations );
           _collision_checker.setFootprint(  
           _costmap_ros->getRobotFootprint(   ),  
           _costmap_ros->getUseRadius(   ),  
           findCircumscribedCost(  _costmap_ros ) );
          
           // Initialize A* template
           _a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           std::numeric_limits<int>::max(   ),  
           _max_planning_time,  
           _lookup_table_dim,  
           _angle_quantizations );
          
           // Initialize path smoother
           if (  smooth_path ) {
           SmootherParams params;
           params.get(  node,   name );
           _smoother = std::make_unique<Smoother>(  params );
           _smoother->initialize(  _minimum_turning_radius_global_coords );
           }
          
           // Initialize costmap downsampler
           if (  _downsample_costmap && _downsampling_factor > 1 ) {
           _costmap_downsampler = std::make_unique<CostmapDownsampler>(   );
           std::string topic_name = "downsampled_costmap";
           _costmap_downsampler->on_configure(  
           node,   _global_frame,   topic_name,   _costmap,   _downsampling_factor );
           }
          
           _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>(  "unsmoothed_plan",   1 );
          
           RCLCPP_INFO(  
           _logger,   "Configured plugin %s of type SmacPlannerHybrid with "
           "maximum iterations %i,   and %s. Using motion model: %s.",  
           _name.c_str(   ),   _max_iterations,  
           _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",  
           toString(  _motion_model ).c_str(   ) );
          }
          
     214  void SmacPlannerHybrid::activate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Activating plugin %s of type SmacPlannerHybrid",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_activate(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_activate(   );
           }
           auto node = _node.lock(   );
           // Add callback for dynamic parameters
           _dyn_params_handler = node->add_on_set_parameters_callback(  
           std::bind(  &SmacPlannerHybrid::dynamicParametersCallback,   this,   _1 ) );
          }
          
     229  void SmacPlannerHybrid::deactivate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Deactivating plugin %s of type SmacPlannerHybrid",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_deactivate(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_deactivate(   );
           }
           _dyn_params_handler.reset(   );
          }
          
     241  void SmacPlannerHybrid::cleanup(   )
          {
           RCLCPP_INFO(  
           _logger,   "Cleaning up plugin %s of type SmacPlannerHybrid",  
           _name.c_str(   ) );
           _a_star.reset(   );
           _smoother.reset(   );
           if (  _costmap_downsampler ) {
           _costmap_downsampler->on_cleanup(   );
           _costmap_downsampler.reset(   );
           }
           _raw_plan_publisher.reset(   );
          }
          
     255  nav_msgs::msg::Path SmacPlannerHybrid::createPlan(  
     256   const geometry_msgs::msg::PoseStamped & start,  
     257   const geometry_msgs::msg::PoseStamped & goal )
          {
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
           steady_clock::time_point a = steady_clock::now(   );
          
           std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(  *(  _costmap->getMutex(   ) ) );
          
           // Downsample costmap,   if required
           nav2_costmap_2d::Costmap2D * costmap = _costmap;
           if (  _costmap_downsampler ) {
           costmap = _costmap_downsampler->downsample(  _downsampling_factor );
           _collision_checker.setCostmap(  costmap );
           }
          
           // Set collision checker and costmap information
           _a_star->setCollisionChecker(  &_collision_checker );
          
           // Set starting point,   in A* bin search coordinates
           unsigned int mx,   my;
           costmap->worldToMap(  start.pose.position.x,   start.pose.position.y,   mx,   my );
           double orientation_bin = tf2::getYaw(  start.pose.orientation ) / _angle_bin_size;
           while (  orientation_bin < 0.0 ) {
           orientation_bin += static_cast<float>(  _angle_quantizations );
           }
           // This is needed to handle precision issues
           if (  orientation_bin >= static_cast<float>(  _angle_quantizations ) ) {
           orientation_bin -= static_cast<float>(  _angle_quantizations );
           }
           unsigned int orientation_bin_id = static_cast<unsigned int>(  floor(  orientation_bin ) );
           _a_star->setStart(  mx,   my,   orientation_bin_id );
          
           // Set goal point,   in A* bin search coordinates
           costmap->worldToMap(  goal.pose.position.x,   goal.pose.position.y,   mx,   my );
           orientation_bin = tf2::getYaw(  goal.pose.orientation ) / _angle_bin_size;
           while (  orientation_bin < 0.0 ) {
           orientation_bin += static_cast<float>(  _angle_quantizations );
           }
           // This is needed to handle precision issues
           if (  orientation_bin >= static_cast<float>(  _angle_quantizations ) ) {
           orientation_bin -= static_cast<float>(  _angle_quantizations );
           }
           orientation_bin_id = static_cast<unsigned int>(  floor(  orientation_bin ) );
           _a_star->setGoal(  mx,   my,   orientation_bin_id );
          
           // Setup message
           nav_msgs::msg::Path plan;
           plan.header.stamp = _clock->now(   );
           plan.header.frame_id = _global_frame;
           geometry_msgs::msg::PoseStamped pose;
           pose.header = plan.header;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
          
           // Compute plan
           NodeHybrid::CoordinateVector path;
           int num_iterations = 0;
           std::string error;
           try {
           if (  !_a_star->createPath(  path,   num_iterations,   0.0 ) ) {
           if (  num_iterations < _a_star->getMaxIterations(   ) ) {
           error = std::string(  "no valid path found" );
           } else {
           error = std::string(  "exceeded maximum iterations" );
           }
           }
           } catch (  const std::runtime_error & e ) {
           error = "invalid use: ";
           error += e.what(   );
           }
          
           if (  !error.empty(   ) ) {
           RCLCPP_WARN(  
           _logger,  
           "%s: failed to create plan,   %s.",  
           _name.c_str(   ),   error.c_str(   ) );
           return plan;
           }
          
           // Convert to world coordinates
           plan.poses.reserve(  path.size(   ) );
           for (  int i = path.size(   ) - 1; i >= 0; --i ) {
           pose.pose = getWorldCoords(  path[i].x,   path[i].y,   costmap );
           pose.pose.orientation = getWorldOrientation(  path[i].theta );
           plan.poses.push_back(  pose );
           }
          
           // Publish raw path for debug
           if (  _raw_plan_publisher->get_subscription_count(   ) > 0 ) {
           _raw_plan_publisher->publish(  plan );
           }
          
           // Find how much time we have left to do smoothing
           steady_clock::time_point b = steady_clock::now(   );
           duration<double> time_span = duration_cast<duration<double>>(  b - a );
           double time_remaining = _max_planning_time - static_cast<double>(  time_span.count(   ) );
          
          #ifdef BENCHMARK_TESTING
           std::cout << "It took " << time_span.count(   ) * 1000 <<
           " milliseconds with " << num_iterations << " iterations." << std::endl;
          #endif
          
           // Smooth plan
           if (  _smoother && num_iterations > 1 ) {
           _smoother->smooth(  plan,   costmap,   time_remaining );
           }
          
          #ifdef BENCHMARK_TESTING
           steady_clock::time_point c = steady_clock::now(   );
           duration<double> time_span2 = duration_cast<duration<double>>(  c - b );
           std::cout << "It took " << time_span2.count(   ) * 1000 <<
           " milliseconds to smooth path." << std::endl;
          #endif
          
           return plan;
          }
          
          rcl_interfaces::msg::SetParametersResult
     377  SmacPlannerHybrid::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
          
           bool reinit_collision_checker = false;
           bool reinit_a_star = false;
           bool reinit_downsampler = false;
           bool reinit_smoother = false;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == _name + ".max_planning_time" ) {
           reinit_a_star = true;
           _max_planning_time = parameter.as_double(   );
           } else if (  name == _name + ".lookup_table_size" ) {
           reinit_a_star = true;
           _lookup_table_size = parameter.as_double(   );
           } else if (  name == _name + ".minimum_turning_radius" ) {
           reinit_a_star = true;
           if (  _smoother ) {
           reinit_smoother = true;
           }
           _minimum_turning_radius_global_coords = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".reverse_penalty" ) {
           reinit_a_star = true;
           _search_info.reverse_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".change_penalty" ) {
           reinit_a_star = true;
           _search_info.change_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".non_straight_penalty" ) {
           reinit_a_star = true;
           _search_info.non_straight_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".cost_penalty" ) {
           reinit_a_star = true;
           _search_info.cost_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".analytic_expansion_ratio" ) {
           reinit_a_star = true;
           _search_info.analytic_expansion_ratio = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".analytic_expansion_max_length" ) {
           reinit_a_star = true;
           _search_info.analytic_expansion_max_length =
           static_cast<float>(  parameter.as_double(   ) ) / _costmap->getResolution(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == _name + ".downsample_costmap" ) {
           reinit_downsampler = true;
           _downsample_costmap = parameter.as_bool(   );
           } else if (  name == _name + ".allow_unknown" ) {
           reinit_a_star = true;
           _allow_unknown = parameter.as_bool(   );
           } else if (  name == _name + ".cache_obstacle_heuristic" ) {
           reinit_a_star = true;
           _search_info.cache_obstacle_heuristic = parameter.as_bool(   );
           } else if (  name == _name + ".smooth_path" ) {
           if (  parameter.as_bool(   ) ) {
           reinit_smoother = true;
           } else {
           _smoother.reset(   );
           }
           }
           } else if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == _name + ".downsampling_factor" ) {
           reinit_a_star = true;
           reinit_downsampler = true;
           _downsampling_factor = parameter.as_int(   );
           } else if (  name == _name + ".max_iterations" ) {
           reinit_a_star = true;
           _max_iterations = parameter.as_int(   );
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
           } else if (  name == _name + ".angle_quantization_bins" ) {
           reinit_collision_checker = true;
           reinit_a_star = true;
           int angle_quantizations = parameter.as_int(   );
           _angle_bin_size = 2.0 * M_PI / angle_quantizations;
           _angle_quantizations = static_cast<unsigned int>(  angle_quantizations );
           }
           } else if (  type == ParameterType::PARAMETER_STRING ) {
           if (  name == _name + ".motion_model_for_search" ) {
           reinit_a_star = true;
           _motion_model = fromString(  parameter.as_string(   ) );
           if (  _motion_model == MotionModel::UNKNOWN ) {
           RCLCPP_WARN(  
           _logger,  
           "Unable to get MotionModel search type. Given '%s',   "
           "valid options are MOORE,   VON_NEUMANN,   DUBIN,   REEDS_SHEPP.",  
           _motion_model_for_search.c_str(   ) );
           }
           }
           }
           }
          
           // Re-init if needed with mutex lock (  to avoid re-init while creating a plan )
           if (  reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother ) {
           // convert to grid coordinates
           if (  !_downsample_costmap ) {
           _downsampling_factor = 1;
           }
           _search_info.minimum_turning_radius =
           _minimum_turning_radius_global_coords / (  _costmap->getResolution(   ) * _downsampling_factor );
           _lookup_table_dim =
           static_cast<float>(  _lookup_table_size ) /
           static_cast<float>(  _costmap->getResolution(   ) * _downsampling_factor );
          
           // Make sure its a whole number
           _lookup_table_dim = static_cast<float>(  static_cast<int>(  _lookup_table_dim ) );
          
           // Make sure its an odd number
           if (  static_cast<int>(  _lookup_table_dim ) % 2 == 0 ) {
           RCLCPP_INFO(  
           _logger,  
           "Even sized heuristic lookup table size set %f,   increasing size by 1 to make odd",  
           _lookup_table_dim );
           _lookup_table_dim += 1.0;
           }
          
           // Re-Initialize A* template
           if (  reinit_a_star ) {
           _a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           std::numeric_limits<int>::max(   ),  
           _max_planning_time,  
           _lookup_table_dim,  
           _angle_quantizations );
           }
          
           // Re-Initialize costmap downsampler
           if (  reinit_downsampler ) {
           if (  _downsample_costmap && _downsampling_factor > 1 ) {
           auto node = _node.lock(   );
           std::string topic_name = "downsampled_costmap";
           _costmap_downsampler = std::make_unique<CostmapDownsampler>(   );
           _costmap_downsampler->on_configure(  
           node,   _global_frame,   topic_name,   _costmap,   _downsampling_factor );
           }
           }
          
           // Re-Initialize collision checker
           if (  reinit_collision_checker ) {
           _collision_checker = GridCollisionChecker(  _costmap,   _angle_quantizations );
           _collision_checker.setFootprint(  
           _costmap_ros->getRobotFootprint(   ),  
           _costmap_ros->getUseRadius(   ),  
           findCircumscribedCost(  _costmap_ros ) );
           }
          
           // Re-Initialize smoother
           if (  reinit_smoother ) {
           auto node = _node.lock(   );
           SmootherParams params;
           params.get(  node,   _name );
           _smoother = std::make_unique<Smoother>(  params );
           _smoother->initialize(  _minimum_turning_radius_global_coords );
           }
           }
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_smac_planner
          
          #include "pluginlib/class_list_macros.hpp"
     549  PLUGINLIB_EXPORT_CLASS(  nav2_smac_planner::SmacPlannerHybrid,   nav2_core::GlobalPlanner )

navigation2/nav2_smac_planner/src/smac_planner_lattice.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <vector>
          #include <algorithm>
          #include <limits>
          
          #include "Eigen/Core"
          #include "nav2_smac_planner/smac_planner_lattice.hpp"
          
          // #define BENCHMARK_TESTING
          
          namespace nav2_smac_planner
          {
          
          using namespace std::chrono; // NOLINT
          using rcl_interfaces::msg::ParameterType;
          
      32  SmacPlannerLattice::SmacPlannerLattice(   )
          : _a_star(  nullptr ),  
           _collision_checker(  nullptr,   1 ),  
           _smoother(  nullptr ),  
           _costmap(  nullptr )
          {
          }
          
      40  SmacPlannerLattice::~SmacPlannerLattice(   )
          {
           RCLCPP_INFO(  
           _logger,   "Destroying plugin %s of type SmacPlannerLattice",  
           _name.c_str(   ) );
          }
          
      47  void SmacPlannerLattice::configure(  
      48   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      49   std::string name,   std::shared_ptr<tf2_ros::Buffer>/*tf*/,  
      50   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           _node = parent;
           auto node = parent.lock(   );
           _logger = node->get_logger(   );
           _clock = node->get_clock(   );
           _costmap = costmap_ros->getCostmap(   );
           _costmap_ros = costmap_ros;
           _name = name;
           _global_frame = costmap_ros->getGlobalFrameID(   );
           _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>(  "unsmoothed_plan",   1 );
          
           RCLCPP_INFO(  _logger,   "Configuring %s of type SmacPlannerLattice",   name.c_str(   ) );
          
           // General planner params
           double analytic_expansion_max_length_m;
           bool smooth_path;
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".allow_unknown",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".allow_unknown",   _allow_unknown );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_iterations",   rclcpp::ParameterValue(  1000000 ) );
           node->get_parameter(  name + ".max_iterations",   _max_iterations );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".smooth_path",   rclcpp::ParameterValue(  true ) );
           node->get_parameter(  name + ".smooth_path",   smooth_path );
          
           // Default to a well rounded model: 16 bin,   0.4m turning radius,   ackermann model
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".lattice_filepath",   rclcpp::ParameterValue(  
           ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" ) +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json" ) );
           node->get_parameter(  name + ".lattice_filepath",   _search_info.lattice_filepath );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".cache_obstacle_heuristic",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".cache_obstacle_heuristic",   _search_info.cache_obstacle_heuristic );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".reverse_penalty",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name + ".reverse_penalty",   _search_info.reverse_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".change_penalty",   rclcpp::ParameterValue(  0.05 ) );
           node->get_parameter(  name + ".change_penalty",   _search_info.change_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".non_straight_penalty",   rclcpp::ParameterValue(  1.05 ) );
           node->get_parameter(  name + ".non_straight_penalty",   _search_info.non_straight_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".cost_penalty",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name + ".cost_penalty",   _search_info.cost_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".retrospective_penalty",   rclcpp::ParameterValue(  0.015 ) );
           node->get_parameter(  name + ".retrospective_penalty",   _search_info.retrospective_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".rotation_penalty",   rclcpp::ParameterValue(  5.0 ) );
           node->get_parameter(  name + ".rotation_penalty",   _search_info.rotation_penalty );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".analytic_expansion_ratio",   rclcpp::ParameterValue(  3.5 ) );
           node->get_parameter(  name + ".analytic_expansion_ratio",   _search_info.analytic_expansion_ratio );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".analytic_expansion_max_length",   rclcpp::ParameterValue(  3.0 ) );
           node->get_parameter(  name + ".analytic_expansion_max_length",   analytic_expansion_max_length_m );
           _search_info.analytic_expansion_max_length =
           analytic_expansion_max_length_m / _costmap->getResolution(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".max_planning_time",   rclcpp::ParameterValue(  5.0 ) );
           node->get_parameter(  name + ".max_planning_time",   _max_planning_time );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".lookup_table_size",   rclcpp::ParameterValue(  20.0 ) );
           node->get_parameter(  name + ".lookup_table_size",   _lookup_table_size );
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".allow_reverse_expansion",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".allow_reverse_expansion",   _search_info.allow_reverse_expansion );
          
           _metadata = LatticeMotionTable::getLatticeMetadata(  _search_info.lattice_filepath );
           _search_info.minimum_turning_radius =
           _metadata.min_turning_radius / (  _costmap->getResolution(   ) );
           _motion_model = MotionModel::STATE_LATTICE;
          
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
          
           float lookup_table_dim =
           static_cast<float>(  _lookup_table_size ) /
           static_cast<float>(  _costmap->getResolution(   ) );
          
           // Make sure its a whole number
           lookup_table_dim = static_cast<float>(  static_cast<int>(  lookup_table_dim ) );
          
           // Make sure its an odd number
           if (  static_cast<int>(  lookup_table_dim ) % 2 == 0 ) {
           RCLCPP_INFO(  
           _logger,  
           "Even sized heuristic lookup table size set %f,   increasing size by 1 to make odd",  
           lookup_table_dim );
           lookup_table_dim += 1.0;
           }
          
           // Initialize collision checker using 72 evenly sized bins instead of the lattice
           // heading angles. This is done so that we have precomputed angles every 5 degrees.
           // If we used the sparse lattice headings (  usually 16 ),   then when we attempt to collision
           // check for intermediary points of the primitives,   we're forced to round to one of the 16
           // increments causing "wobbly" checks that could cause larger robots to virtually show collisions
           // in valid configurations. This approximation helps to bound orientation error for all checks
           // in exchange for slight inaccuracies in the collision headings in terminal search states.
           _collision_checker = GridCollisionChecker(  _costmap,   72u );
           _collision_checker.setFootprint(  
           costmap_ros->getRobotFootprint(   ),  
           costmap_ros->getUseRadius(   ),  
           findCircumscribedCost(  costmap_ros ) );
          
           // Initialize A* template
           _a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           std::numeric_limits<int>::max(   ),  
           _max_planning_time,  
           lookup_table_dim,  
           _metadata.number_of_headings );
          
           // Initialize path smoother
           if (  smooth_path ) {
           SmootherParams params;
           params.get(  node,   name );
           _smoother = std::make_unique<Smoother>(  params );
           _smoother->initialize(  _metadata.min_turning_radius );
           }
          
           RCLCPP_INFO(  
           _logger,   "Configured plugin %s of type SmacPlannerLattice with "
           "maximum iterations %i,   "
           "and %s. Using motion model: %s. State lattice file: %s.",  
           _name.c_str(   ),   _max_iterations,  
           _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",  
           toString(  _motion_model ).c_str(   ),   _search_info.lattice_filepath.c_str(   ) );
          }
          
     192  void SmacPlannerLattice::activate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Activating plugin %s of type SmacPlannerLattice",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_activate(   );
           auto node = _node.lock(   );
           // Add callback for dynamic parameters
           _dyn_params_handler = node->add_on_set_parameters_callback(  
           std::bind(  &SmacPlannerLattice::dynamicParametersCallback,   this,   std::placeholders::_1 ) );
          }
          
     204  void SmacPlannerLattice::deactivate(   )
          {
           RCLCPP_INFO(  
           _logger,   "Deactivating plugin %s of type SmacPlannerLattice",  
           _name.c_str(   ) );
           _raw_plan_publisher->on_deactivate(   );
           _dyn_params_handler.reset(   );
          }
          
     213  void SmacPlannerLattice::cleanup(   )
          {
           RCLCPP_INFO(  
           _logger,   "Cleaning up plugin %s of type SmacPlannerLattice",  
           _name.c_str(   ) );
           _a_star.reset(   );
           _smoother.reset(   );
           _raw_plan_publisher.reset(   );
          }
          
     223  nav_msgs::msg::Path SmacPlannerLattice::createPlan(  
     224   const geometry_msgs::msg::PoseStamped & start,  
     225   const geometry_msgs::msg::PoseStamped & goal )
          {
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
           steady_clock::time_point a = steady_clock::now(   );
          
           std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(  *(  _costmap->getMutex(   ) ) );
          
           // Set collision checker and costmap information
           _a_star->setCollisionChecker(  &_collision_checker );
          
           // Set starting point,   in A* bin search coordinates
           unsigned int mx,   my;
           _costmap->worldToMap(  start.pose.position.x,   start.pose.position.y,   mx,   my );
           _a_star->setStart(  
           mx,   my,  
           NodeLattice::motion_table.getClosestAngularBin(  tf2::getYaw(  start.pose.orientation ) ) );
          
           // Set goal point,   in A* bin search coordinates
           _costmap->worldToMap(  goal.pose.position.x,   goal.pose.position.y,   mx,   my );
           _a_star->setGoal(  
           mx,   my,  
           NodeLattice::motion_table.getClosestAngularBin(  tf2::getYaw(  goal.pose.orientation ) ) );
          
           // Setup message
           nav_msgs::msg::Path plan;
           plan.header.stamp = _clock->now(   );
           plan.header.frame_id = _global_frame;
           geometry_msgs::msg::PoseStamped pose;
           pose.header = plan.header;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
          
           // Compute plan
           NodeLattice::CoordinateVector path;
           int num_iterations = 0;
           std::string error;
           try {
           if (  !_a_star->createPath(  path,   num_iterations,   0 /*no tolerance*/ ) ) {
           if (  num_iterations < _a_star->getMaxIterations(   ) ) {
           error = std::string(  "no valid path found" );
           } else {
           error = std::string(  "exceeded maximum iterations" );
           }
           }
           } catch (  const std::runtime_error & e ) {
           error = "invalid use: ";
           error += e.what(   );
           }
          
           if (  !error.empty(   ) ) {
           RCLCPP_WARN(  
           _logger,  
           "%s: failed to create plan,   %s.",  
           _name.c_str(   ),   error.c_str(   ) );
           return plan;
           }
          
           // Convert to world coordinates
           plan.poses.reserve(  path.size(   ) );
           geometry_msgs::msg::PoseStamped last_pose = pose;
           for (  int i = path.size(   ) - 1; i >= 0; --i ) {
           pose.pose = getWorldCoords(  path[i].x,   path[i].y,   _costmap );
           pose.pose.orientation = getWorldOrientation(  path[i].theta );
           if (  fabs(  pose.pose.position.x - last_pose.pose.position.x ) < 1e-4 &&
           fabs(  pose.pose.position.y - last_pose.pose.position.y ) < 1e-4 &&
           fabs(  tf2::getYaw(  pose.pose.orientation ) - tf2::getYaw(  last_pose.pose.orientation ) ) < 1e-4 )
           {
           RCLCPP_DEBUG(  
           _logger,  
           "Removed a path from the path due to replication. "
           "Make sure your minimum control set does not contain duplicate values!" );
           continue;
           }
           last_pose = pose;
           plan.poses.push_back(  pose );
           }
          
           // Publish raw path for debug
           if (  _raw_plan_publisher->get_subscription_count(   ) > 0 ) {
           _raw_plan_publisher->publish(  plan );
           }
          
           // Find how much time we have left to do smoothing
           steady_clock::time_point b = steady_clock::now(   );
           duration<double> time_span = duration_cast<duration<double>>(  b - a );
           double time_remaining = _max_planning_time - static_cast<double>(  time_span.count(   ) );
          
          #ifdef BENCHMARK_TESTING
           std::cout << "It took " << time_span.count(   ) * 1000 <<
           " milliseconds with " << num_iterations << " iterations." << std::endl;
          #endif
          
           // Smooth plan
           if (  _smoother && num_iterations > 1 ) {
           _smoother->smooth(  plan,   _costmap,   time_remaining );
           }
          
          #ifdef BENCHMARK_TESTING
           steady_clock::time_point c = steady_clock::now(   );
           duration<double> time_span2 = duration_cast<duration<double>>(  c - b );
           std::cout << "It took " << time_span2.count(   ) * 1000 <<
           " milliseconds to smooth path." << std::endl;
          #endif
          
           return plan;
          }
          
          rcl_interfaces::msg::SetParametersResult
     336  SmacPlannerLattice::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           std::lock_guard<std::mutex> lock_reinit(  _mutex );
          
           bool reinit_a_star = false;
           bool reinit_smoother = false;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == _name + ".max_planning_time" ) {
           reinit_a_star = true;
           _max_planning_time = parameter.as_double(   );
           } else if (  name == _name + ".lookup_table_size" ) {
           reinit_a_star = true;
           _lookup_table_size = parameter.as_double(   );
           } else if (  name == _name + ".reverse_penalty" ) {
           reinit_a_star = true;
           _search_info.reverse_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".change_penalty" ) {
           reinit_a_star = true;
           _search_info.change_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".non_straight_penalty" ) {
           reinit_a_star = true;
           _search_info.non_straight_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".cost_penalty" ) {
           reinit_a_star = true;
           _search_info.cost_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".rotation_penalty" ) {
           reinit_a_star = true;
           _search_info.rotation_penalty = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".analytic_expansion_ratio" ) {
           reinit_a_star = true;
           _search_info.analytic_expansion_ratio = static_cast<float>(  parameter.as_double(   ) );
           } else if (  name == _name + ".analytic_expansion_max_length" ) {
           reinit_a_star = true;
           _search_info.analytic_expansion_max_length =
           static_cast<float>(  parameter.as_double(   ) ) / _costmap->getResolution(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == _name + ".allow_unknown" ) {
           reinit_a_star = true;
           _allow_unknown = parameter.as_bool(   );
           } else if (  name == _name + ".cache_obstacle_heuristic" ) {
           reinit_a_star = true;
           _search_info.cache_obstacle_heuristic = parameter.as_bool(   );
           } else if (  name == _name + ".allow_reverse_expansion" ) {
           reinit_a_star = true;
           _search_info.allow_reverse_expansion = parameter.as_bool(   );
           } else if (  name == _name + ".smooth_path" ) {
           if (  parameter.as_bool(   ) ) {
           reinit_smoother = true;
           } else {
           _smoother.reset(   );
           }
           }
           } else if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == _name + ".max_iterations" ) {
           reinit_a_star = true;
           _max_iterations = parameter.as_int(   );
           if (  _max_iterations <= 0 ) {
           RCLCPP_INFO(  
           _logger,   "maximum iteration selected as <= 0,   "
           "disabling maximum iterations." );
           _max_iterations = std::numeric_limits<int>::max(   );
           }
           }
           } else if (  type == ParameterType::PARAMETER_STRING ) {
           if (  name == _name + ".lattice_filepath" ) {
           reinit_a_star = true;
           if (  _smoother ) {
           reinit_smoother = true;
           }
           _search_info.lattice_filepath = parameter.as_string(   );
           _metadata = LatticeMotionTable::getLatticeMetadata(  _search_info.lattice_filepath );
           _search_info.minimum_turning_radius =
           _metadata.min_turning_radius / (  _costmap->getResolution(   ) );
           }
           }
           }
          
           // Re-init if needed with mutex lock (  to avoid re-init while creating a plan )
           if (  reinit_a_star || reinit_smoother ) {
           // convert to grid coordinates
           _search_info.minimum_turning_radius =
           _metadata.min_turning_radius / (  _costmap->getResolution(   ) );
           float lookup_table_dim =
           static_cast<float>(  _lookup_table_size ) /
           static_cast<float>(  _costmap->getResolution(   ) );
          
           // Make sure its a whole number
           lookup_table_dim = static_cast<float>(  static_cast<int>(  lookup_table_dim ) );
          
           // Make sure its an odd number
           if (  static_cast<int>(  lookup_table_dim ) % 2 == 0 ) {
           RCLCPP_INFO(  
           _logger,  
           "Even sized heuristic lookup table size set %f,   increasing size by 1 to make odd",  
           lookup_table_dim );
           lookup_table_dim += 1.0;
           }
          
           // Re-Initialize smoother
           if (  reinit_smoother ) {
           auto node = _node.lock(   );
           SmootherParams params;
           params.get(  node,   _name );
           _smoother = std::make_unique<Smoother>(  params );
           _smoother->initialize(  _metadata.min_turning_radius );
           }
          
           // Re-Initialize A* template
           if (  reinit_a_star ) {
           _a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(  _motion_model,   _search_info );
           _a_star->initialize(  
           _allow_unknown,  
           _max_iterations,  
           std::numeric_limits<int>::max(   ),  
           _max_planning_time,  
           lookup_table_dim,  
           _metadata.number_of_headings );
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_smac_planner
          
          #include "pluginlib/class_list_macros.hpp"
     470  PLUGINLIB_EXPORT_CLASS(  nav2_smac_planner::SmacPlannerLattice,   nav2_core::GlobalPlanner )

navigation2/nav2_smac_planner/src/smoother.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <ompl/base/ScopedState.h>
          #include <ompl/base/spaces/DubinsStateSpace.h>
          #include <vector>
          #include <memory>
          #include "nav2_smac_planner/smoother.hpp"
          
          namespace nav2_smac_planner
          {
          using namespace nav2_util::geometry_utils; // NOLINT
          using namespace std::chrono; // NOLINT
          
      26  Smoother::Smoother(  const SmootherParams & params )
          {
           tolerance_ = params.tolerance_;
           max_its_ = params.max_its_;
           data_w_ = params.w_data_;
           smooth_w_ = params.w_smooth_;
           is_holonomic_ = params.holonomic_;
           do_refinement_ = params.do_refinement_;
          }
          
      36  void Smoother::initialize(  const double & min_turning_radius )
          {
           min_turning_rad_ = min_turning_radius;
           state_space_ = std::make_unique<ompl::base::DubinsStateSpace>(  min_turning_rad_ );
          }
          
      42  bool Smoother::smooth(  
      43   nav_msgs::msg::Path & path,  
      44   const nav2_costmap_2d::Costmap2D * costmap,  
           const double & max_time )
          {
           // by-pass path orientations approximation when skipping smac smoother
           if (  max_its_ == 0 ) {
           return false;
           }
          
           refinement_ctr_ = 0;
           steady_clock::time_point start = steady_clock::now(   );
           double time_remaining = max_time;
           bool success = true,   reversing_segment;
           nav_msgs::msg::Path curr_path_segment;
           curr_path_segment.header = path.header;
           std::vector<PathSegment> path_segments = findDirectionalPathSegments(  path );
          
           for (  unsigned int i = 0; i != path_segments.size(   ); i++ ) {
           if (  path_segments[i].end - path_segments[i].start > 10 ) {
           // Populate path segment
           curr_path_segment.poses.clear(   );
           std::copy(  
           path.poses.begin(   ) + path_segments[i].start,  
           path.poses.begin(   ) + path_segments[i].end + 1,  
           std::back_inserter(  curr_path_segment.poses ) );
          
           // Make sure we're still able to smooth with time remaining
           steady_clock::time_point now = steady_clock::now(   );
           time_remaining = max_time - duration_cast<duration<double>>(  now - start ).count(   );
          
           // Smooth path segment naively
           const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front(   ).pose;
           const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back(   ).pose;
           bool local_success =
           smoothImpl(  curr_path_segment,   reversing_segment,   costmap,   time_remaining );
           success = success && local_success;
          
           // Enforce boundary conditions
           if (  !is_holonomic_ && local_success ) {
           enforceStartBoundaryConditions(  start_pose,   curr_path_segment,   costmap,   reversing_segment );
           enforceEndBoundaryConditions(  goal_pose,   curr_path_segment,   costmap,   reversing_segment );
           }
          
           // Assemble the path changes to the main path
           std::copy(  
           curr_path_segment.poses.begin(   ),  
           curr_path_segment.poses.end(   ),  
           path.poses.begin(   ) + path_segments[i].start );
           }
           }
          
           return success;
          }
          
      97  bool Smoother::smoothImpl(  
      98   nav_msgs::msg::Path & path,  
      99   bool & reversing_segment,  
     100   const nav2_costmap_2d::Costmap2D * costmap,  
           const double & max_time )
          {
           steady_clock::time_point a = steady_clock::now(   );
           rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(  max_time );
          
           int its = 0;
           double change = tolerance_;
           const unsigned int & path_size = path.poses.size(   );
           double x_i,   y_i,   y_m1,   y_ip1,   y_i_org;
           unsigned int mx,   my;
          
           nav_msgs::msg::Path new_path = path;
           nav_msgs::msg::Path last_path = path;
          
           while (  change >= tolerance_ ) {
           its += 1;
           change = 0.0;
          
           // Make sure the smoothing function will converge
           if (  its >= max_its_ ) {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "SmacPlannerSmoother" ),  
           "Number of iterations has exceeded limit of %i.",   max_its_ );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
          
           // Make sure still have time left to process
           steady_clock::time_point b = steady_clock::now(   );
           rclcpp::Duration timespan(  duration_cast<duration<double>>(  b - a ) );
           if (  timespan > max_dur ) {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "SmacPlannerSmoother" ),  
           "Smoothing time exceeded allowed duration of %0.2f.",   max_time );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
          
           for (  unsigned int i = 1; i != path_size - 1; i++ ) {
           for (  unsigned int j = 0; j != 2; j++ ) {
           x_i = getFieldByDim(  path.poses[i],   j );
           y_i = getFieldByDim(  new_path.poses[i],   j );
           y_m1 = getFieldByDim(  new_path.poses[i - 1],   j );
           y_ip1 = getFieldByDim(  new_path.poses[i + 1],   j );
           y_i_org = y_i;
          
           // Smooth based on local 3 point neighborhood and original data locations
           y_i += data_w_ * (  x_i - y_i ) + smooth_w_ * (  y_ip1 + y_m1 - (  2.0 * y_i ) );
           setFieldByDim(  new_path.poses[i],   j,   y_i );
           change += abs(  y_i - y_i_org );
           }
          
           // validate update is admissible,   only checks cost if a valid costmap pointer is provided
           float cost = 0.0;
           if (  costmap ) {
           costmap->worldToMap(  
           getFieldByDim(  new_path.poses[i],   0 ),  
           getFieldByDim(  new_path.poses[i],   1 ),  
           mx,   my );
           cost = static_cast<float>(  costmap->getCost(  mx,   my ) );
           }
          
           if (  cost > MAX_NON_OBSTACLE && cost != UNKNOWN ) {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "SmacPlannerSmoother" ),  
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced." );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
           }
          
           last_path = new_path;
           }
          
           // Lets do additional refinement,   it shouldn't take more than a couple milliseconds
           // but really puts the path quality over the top.
           if (  do_refinement_ && refinement_ctr_ < 4 ) {
           refinement_ctr_++;
           smoothImpl(  new_path,   reversing_segment,   costmap,   max_time );
           }
          
           updateApproximatePathOrientations(  new_path,   reversing_segment );
           path = new_path;
           return true;
          }
          
     191  double Smoother::getFieldByDim(  
     192   const geometry_msgs::msg::PoseStamped & msg,   const unsigned int & dim )
          {
           if (  dim == 0 ) {
           return msg.pose.position.x;
           } else if (  dim == 1 ) {
           return msg.pose.position.y;
           } else {
           return msg.pose.position.z;
           }
          }
          
     203  void Smoother::setFieldByDim(  
     204   geometry_msgs::msg::PoseStamped & msg,   const unsigned int dim,  
           const double & value )
          {
           if (  dim == 0 ) {
           msg.pose.position.x = value;
           } else if (  dim == 1 ) {
           msg.pose.position.y = value;
           } else {
           msg.pose.position.z = value;
           }
          }
          
     216  std::vector<PathSegment> Smoother::findDirectionalPathSegments(  const nav_msgs::msg::Path & path )
          {
           std::vector<PathSegment> segments;
           PathSegment curr_segment;
           curr_segment.start = 0;
          
           // If holonomic,   no directional changes and
           // may have abrupt angular changes from naive grid search
           if (  is_holonomic_ ) {
           curr_segment.end = path.poses.size(   ) - 1;
           segments.push_back(  curr_segment );
           return segments;
           }
          
           // Iterating through the path to determine the position of the cusp
           for (  unsigned int idx = 1; idx < path.poses.size(   ) - 1; ++idx ) {
           // We have two vectors for the dot product OA and AB. Determining the vectors.
           double oa_x = path.poses[idx].pose.position.x -
           path.poses[idx - 1].pose.position.x;
           double oa_y = path.poses[idx].pose.position.y -
           path.poses[idx - 1].pose.position.y;
           double ab_x = path.poses[idx + 1].pose.position.x -
           path.poses[idx].pose.position.x;
           double ab_y = path.poses[idx + 1].pose.position.y -
           path.poses[idx].pose.position.y;
          
           // Checking for the existance of cusp,   in the path,   using the dot product.
           double dot_product = (  oa_x * ab_x ) + (  oa_y * ab_y );
           if (  dot_product < 0.0 ) {
           curr_segment.end = idx;
           segments.push_back(  curr_segment );
           curr_segment.start = idx;
           }
          
           // Checking for the existance of a differential rotation in place.
           double cur_theta = tf2::getYaw(  path.poses[idx].pose.orientation );
           double next_theta = tf2::getYaw(  path.poses[idx + 1].pose.orientation );
           double dtheta = angles::shortest_angular_distance(  cur_theta,   next_theta );
           if (  fabs(  ab_x ) < 1e-4 && fabs(  ab_y ) < 1e-4 && fabs(  dtheta ) > 1e-4 ) {
           curr_segment.end = idx;
           segments.push_back(  curr_segment );
           curr_segment.start = idx;
           }
           }
          
           curr_segment.end = path.poses.size(   ) - 1;
           segments.push_back(  curr_segment );
           return segments;
          }
          
     266  void Smoother::updateApproximatePathOrientations(  
     267   nav_msgs::msg::Path & path,  
     268   bool & reversing_segment )
          {
           double dx,   dy,   theta,   pt_yaw;
           reversing_segment = false;
          
           // Find if this path segment is in reverse
           dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
           dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
           theta = atan2(  dy,   dx );
           pt_yaw = tf2::getYaw(  path.poses[1].pose.orientation );
           if (  !is_holonomic_ && fabs(  angles::shortest_angular_distance(  pt_yaw,   theta ) ) > M_PI_2 ) {
           reversing_segment = true;
           }
          
           // Find the angle relative the path position vectors
           for (  unsigned int i = 0; i != path.poses.size(   ) - 1; i++ ) {
           dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
           dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
           theta = atan2(  dy,   dx );
          
           // If points are overlapping,   pass
           if (  fabs(  dx ) < 1e-4 && fabs(  dy ) < 1e-4 ) {
           continue;
           }
          
           // Flip the angle if this path segment is in reverse
           if (  reversing_segment ) {
           theta += M_PI; // orientationAroundZAxis will normalize
           }
          
           path.poses[i].pose.orientation = orientationAroundZAxis(  theta );
           }
          }
          
     302  unsigned int Smoother::findShortestBoundaryExpansionIdx(  
     303   const BoundaryExpansions & boundary_expansions )
          {
           // Check which is valid with the minimum integrated length such that
           // shorter end-points away that are infeasible to achieve without
           // a loop-de-loop are punished
           double min_length = 1e9;
           int shortest_boundary_expansion_idx = 1e9;
           for (  unsigned int idx = 0; idx != boundary_expansions.size(   ); idx++ ) {
           if (  boundary_expansions[idx].expansion_path_length<min_length &&
           !boundary_expansions[idx].in_collision &&
           boundary_expansions[idx].path_end_idx>0.0 &&
           boundary_expansions[idx].expansion_path_length > 0.0 )
           {
           min_length = boundary_expansions[idx].expansion_path_length;
           shortest_boundary_expansion_idx = idx;
           }
           }
          
           return shortest_boundary_expansion_idx;
          }
          
     324  void Smoother::findBoundaryExpansion(  
     325   const geometry_msgs::msg::Pose & start,  
     326   const geometry_msgs::msg::Pose & end,  
           BoundaryExpansion & expansion,  
     328   const nav2_costmap_2d::Costmap2D * costmap )
          {
           static ompl::base::ScopedState<> from(  state_space_ ),   to(  state_space_ ),   s(  state_space_ );
          
           from[0] = start.position.x;
           from[1] = start.position.y;
           from[2] = tf2::getYaw(  start.orientation );
           to[0] = end.position.x;
           to[1] = end.position.y;
           to[2] = tf2::getYaw(  end.orientation );
          
           double d = state_space_->distance(  from(   ),   to(   ) );
           // If this path is too long compared to the original,   then this is probably
           // a loop-de-loop,   treat as invalid as to not deviate too far from the original path.
           // 2.0 selected from prinicipled choice of boundary test points
           // r,   2 * r,   r * PI,   and 2 * PI * r. If there is a loop,   it will be
           // approximately 2 * PI * r,   which is 2 * PI > r,   PI > 2 * r,   and 2 > r * PI.
           // For all but the last backup test point,   a loop would be approximately
           // 2x greater than any of the selections.
           if (  d > 2.0 * expansion.original_path_length ) {
           return;
           }
          
           std::vector<double> reals;
           double theta(  0.0 ),   x(  0.0 ),   y(  0.0 );
           double x_m = start.position.x;
           double y_m = start.position.y;
          
           // Get intermediary poses
           for (  double i = 0; i <= expansion.path_end_idx; i++ ) {
           state_space_->interpolate(  from(   ),   to(   ),   i / expansion.path_end_idx,   s(   ) );
           reals = s.reals(   );
           // Make sure in range [0,   2PI )
           theta = (  reals[2] < 0.0 ) ? (  reals[2] + 2.0 * M_PI ) : reals[2];
           theta = (  theta > 2.0 * M_PI ) ? (  theta - 2.0 * M_PI ) : theta;
           x = reals[0];
           y = reals[1];
          
           // Check for collision
           unsigned int mx,   my;
           costmap->worldToMap(  x,   y,   mx,   my );
           if (  static_cast<float>(  costmap->getCost(  mx,   my ) ) >= INSCRIBED ) {
           expansion.in_collision = true;
           }
          
           // Integrate path length
           expansion.expansion_path_length += hypot(  x - x_m,   y - y_m );
           x_m = x;
           y_m = y;
          
           // Store point
           expansion.pts.emplace_back(  x,   y,   theta );
           }
          }
          
          template<typename IteratorT>
     384  BoundaryExpansions Smoother::generateBoundaryExpansionPoints(  IteratorT start,   IteratorT end )
          {
           std::vector<double> distances = {
           min_turning_rad_,   // Radius
           2.0 * min_turning_rad_,   // Diameter
           M_PI * min_turning_rad_,   // 50% Circumference
           2.0 * M_PI * min_turning_rad_ // Circumference
           };
          
           BoundaryExpansions boundary_expansions;
           boundary_expansions.resize(  distances.size(   ) );
           double curr_dist = 0.0;
           double x_last = start->pose.position.x;
           double y_last = start->pose.position.y;
           geometry_msgs::msg::Point pt;
           unsigned int curr_dist_idx = 0;
          
           for (  IteratorT iter = start; iter != end; iter++ ) {
           pt = iter->pose.position;
           curr_dist += hypot(  pt.x - x_last,   pt.y - y_last );
           x_last = pt.x;
           y_last = pt.y;
          
           if (  curr_dist >= distances[curr_dist_idx] ) {
           boundary_expansions[curr_dist_idx].path_end_idx = iter - start;
           boundary_expansions[curr_dist_idx].original_path_length = curr_dist;
           curr_dist_idx++;
           }
          
           if (  curr_dist_idx == boundary_expansions.size(   ) ) {
           break;
           }
           }
          
           return boundary_expansions;
          }
          
     421  void Smoother::enforceStartBoundaryConditions(  
     422   const geometry_msgs::msg::Pose & start_pose,  
     423   nav_msgs::msg::Path & path,  
     424   const nav2_costmap_2d::Costmap2D * costmap,  
     425   const bool & reversing_segment )
          {
           // Find range of points for testing
           BoundaryExpansions boundary_expansions =
           generateBoundaryExpansionPoints<PathIterator>(  path.poses.begin(   ),   path.poses.end(   ) );
          
           // Generate the motion model and metadata from start -> test points
           for (  unsigned int i = 0; i != boundary_expansions.size(   ); i++ ) {
           BoundaryExpansion & expansion = boundary_expansions[i];
           if (  expansion.path_end_idx == 0.0 ) {
           continue;
           }
          
           if (  !reversing_segment ) {
           findBoundaryExpansion(  
           start_pose,   path.poses[expansion.path_end_idx].pose,   expansion,  
           costmap );
           } else {
           findBoundaryExpansion(  
           path.poses[expansion.path_end_idx].pose,   start_pose,   expansion,  
           costmap );
           }
           }
          
           // Find the shortest kinematically feasible boundary expansion
           unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(  boundary_expansions );
           if (  best_expansion_idx > boundary_expansions.size(   ) ) {
           return;
           }
          
           // Override values to match curve
           BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
           if (  reversing_segment ) {
           std::reverse(  best_expansion.pts.begin(   ),   best_expansion.pts.end(   ) );
           }
           for (  unsigned int i = 0; i != best_expansion.pts.size(   ); i++ ) {
           path.poses[i].pose.position.x = best_expansion.pts[i].x;
           path.poses[i].pose.position.y = best_expansion.pts[i].y;
           path.poses[i].pose.orientation = orientationAroundZAxis(  best_expansion.pts[i].theta );
           }
          }
          
     467  void Smoother::enforceEndBoundaryConditions(  
     468   const geometry_msgs::msg::Pose & end_pose,  
     469   nav_msgs::msg::Path & path,  
     470   const nav2_costmap_2d::Costmap2D * costmap,  
     471   const bool & reversing_segment )
          {
           // Find range of points for testing
           BoundaryExpansions boundary_expansions =
           generateBoundaryExpansionPoints<ReversePathIterator>(  path.poses.rbegin(   ),   path.poses.rend(   ) );
          
           // Generate the motion model and metadata from start -> test points
           unsigned int expansion_starting_idx;
           for (  unsigned int i = 0; i != boundary_expansions.size(   ); i++ ) {
           BoundaryExpansion & expansion = boundary_expansions[i];
           if (  expansion.path_end_idx == 0.0 ) {
           continue;
           }
           expansion_starting_idx = path.poses.size(   ) - expansion.path_end_idx - 1;
           if (  !reversing_segment ) {
           findBoundaryExpansion(  path.poses[expansion_starting_idx].pose,   end_pose,   expansion,   costmap );
           } else {
           findBoundaryExpansion(  end_pose,   path.poses[expansion_starting_idx].pose,   expansion,   costmap );
           }
           }
          
           // Find the shortest kinematically feasible boundary expansion
           unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(  boundary_expansions );
           if (  best_expansion_idx > boundary_expansions.size(   ) ) {
           return;
           }
          
           // Override values to match curve
           BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
           if (  reversing_segment ) {
           std::reverse(  best_expansion.pts.begin(   ),   best_expansion.pts.end(   ) );
           }
           expansion_starting_idx = path.poses.size(   ) - best_expansion.path_end_idx - 1;
           for (  unsigned int i = 0; i != best_expansion.pts.size(   ); i++ ) {
           path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x;
           path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y;
           path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis(  
           best_expansion.pts[i].theta );
           }
          }
          
          } // namespace nav2_smac_planner

navigation2/nav2_smac_planner/test/deprecated/smoother.hpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef DEPRECATED__SMOOTHER_HPP_
          #define DEPRECATED__SMOOTHER_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/smoother_cost_function.hpp"
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::Smoother
           * @brief A Conjugate Gradient 2D path smoother implementation
           */
      38  class Smoother
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::Smoother
           */
      44   Smoother(   ) {}
          
           /**
           * @brief A destructor for nav2_smac_planner::Smoother
           */
      49   ~Smoother(   ) {}
          
           /**
           * @brief Initialization of the smoother
           * @param params OptimizerParam struct
           */
      55   void initialize(  const OptimizerParams params )
           {
           _debug = params.debug;
          
           // General Params
          
           // 2 most valid options: STEEPEST_DESCENT,   NONLINEAR_CONJUGATE_GRADIENT
           _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
           _options.line_search_type = ceres::WOLFE;
           _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
           _options.line_search_interpolation_type = ceres::CUBIC;
          
           _options.max_num_iterations = params.max_iterations;
           _options.max_solver_time_in_seconds = params.max_time;
          
           _options.function_tolerance = params.fn_tol;
           _options.gradient_tolerance = params.gradient_tol;
           _options.parameter_tolerance = params.param_tol;
          
           _options.min_line_search_step_size = params.advanced.min_line_search_step_size;
           _options.max_num_line_search_step_size_iterations =
           params.advanced.max_num_line_search_step_size_iterations;
           _options.line_search_sufficient_function_decrease =
           params.advanced.line_search_sufficient_function_decrease;
           _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
           _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
           _options.max_num_line_search_direction_restarts =
           params.advanced.max_num_line_search_direction_restarts;
           _options.line_search_sufficient_curvature_decrease =
           params.advanced.line_search_sufficient_curvature_decrease;
           _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
          
           if (  _debug ) {
           _options.minimizer_progress_to_stdout = true;
           } else {
           _options.logging_type = ceres::SILENT;
           }
           }
          
           /**
           * @brief Smoother method
           * @param path Reference to path
           * @param costmap Pointer to minimal costmap
           * @param smoother parameters weights
           * @return If smoothing was successful
           */
     101   bool smooth(  
     102   std::vector<Eigen::Vector2d> & path,  
     103   nav2_costmap_2d::Costmap2D * costmap,  
           const SmootherParams & params )
           {
           _options.max_solver_time_in_seconds = params.max_time;
          
          #ifdef _MSC_VER
           std::vector<double> parameters_vec(  path.size(   ) * 2 );
           double * parameters = parameters_vec.data(   );
          #else
           double parameters[path.size(   ) * 2]; // NOLINT
          #endif
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           parameters[2 * i] = path[i][0];
           parameters[2 * i + 1] = path[i][1];
           }
          
           ceres::GradientProblemSolver::Summary summary;
           ceres::GradientProblem problem(  new UnconstrainedSmootherCostFunction(  &path,   costmap,   params ) );
           ceres::Solve(  _options,   problem,   parameters,   &summary );
          
           if (  _debug ) {
           std::cout << summary.FullReport(   ) << '\n';
           }
          
           if (  !summary.IsSolutionUsable(   ) || summary.initial_cost - summary.final_cost <= 0.0 ) {
           return false;
           }
          
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           path[i][0] = parameters[2 * i];
           path[i][1] = parameters[2 * i + 1];
           }
          
           return true;
           }
          
          private:
     140   bool _debug;
     141   ceres::GradientProblemSolver::Options _options;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // DEPRECATED__SMOOTHER_HPP_

navigation2/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
          #define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <unordered_map>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_smac_planner/options.hpp"
          
          #define EPSILON 0.0001
          
          namespace nav2_smac_planner
          {
          
          /**
           * @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
           * @brief Cost function for path smoothing with multiple terms
           * including curvature,   smoothness,   collision,   and avoid obstacles.
           */
      42  class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
           * @param original_path Original unsmoothed path to smooth
           * @param costmap A costmap to get values for collision and obstacle avoidance
           */
      50   UnconstrainedSmootherCostFunction(  
      51   std::vector<Eigen::Vector2d> * original_path,  
      52   nav2_costmap_2d::Costmap2D * costmap,  
           const SmootherParams & params )
           : _original_path(  original_path ),  
           _num_params(  2 * original_path->size(   ) ),  
           _costmap(  costmap ),  
           _params(  params )
           {
           // int height = costmap->getSizeInCellsX(   );
           // int width = costmap->getSizeInCellsY(   );
           // bool** binMap;
           // binMap = new bool*[width];
          
           // for (  int x = 0; x < width; x++ ) { binMap[x] = new bool[height]; }
          
           // for (  int x = 0; x < width; ++x ) {
           // for (  int y = 0; y < height; ++y ) {
           // binMap[x][y] = costmap->getCost(  x,  y ) >= 253 ? true : false;
           // }
           // }
          
           // voronoiDiagram.initializeMap(  width,   height,   binMap );
           // voronoiDiagram.update(   );
           // voronoiDiagram.visualize(   );
           }
          
           /**
           * @struct CurvatureComputations
           * @brief Cache common computations between the curvature terms to minimize recomputations
           */
           struct CurvatureComputations
           {
           /**
           * @brief A constructor for nav2_smac_planner::CurvatureComputations
           */
           CurvatureComputations(   )
           {
           valid = true;
           }
          
           bool valid;
           /**
           * @brief Check if result is valid for penalty
           * @return is valid (  non-nan,   non-inf,   and turning angle > max )
           */
      96   bool isValid(   )
           {
           return valid;
           }
          
           Eigen::Vector2d delta_xi{0.0,   0.0};
           Eigen::Vector2d delta_xi_p{0.0,   0.0};
           double delta_xi_norm{0};
           double delta_xi_p_norm{0};
           double delta_phi_i{0};
           double turning_rad{0};
           double ki_minus_kmax{0};
           };
          
           /**
           * @brief Smoother cost function evaluation
           * @param parameters X,  Y pairs of points
           * @param cost total cost of path
           * @param gradient of path at each X,  Y pair from cost function derived analytically
           * @return if successful in computing values
           */
           virtual bool Evaluate(  
           const double * parameters,  
           double * cost,  
           double * gradient ) const
           {
           Eigen::Vector2d xi;
           Eigen::Vector2d xi_p1;
           Eigen::Vector2d xi_m1;
           unsigned int x_index,   y_index;
           cost[0] = 0.0;
           double cost_raw = 0.0;
           double grad_x_raw = 0.0;
           double grad_y_raw = 0.0;
           unsigned int mx,   my;
           bool valid_coords = true;
           double costmap_cost = 0.0;
          
           // cache some computations between the residual and jacobian
           CurvatureComputations curvature_params;
          
           for (  int i = 0; i != NumParameters(   ) / 2; i++ ) {
           x_index = 2 * i;
           y_index = 2 * i + 1;
           gradient[x_index] = 0.0;
           gradient[y_index] = 0.0;
           if (  i < 1 || i >= NumParameters(   ) / 2 - 1 ) {
           continue;
           }
          
           xi = Eigen::Vector2d(  parameters[x_index],   parameters[y_index] );
           xi_p1 = Eigen::Vector2d(  parameters[x_index + 2],   parameters[y_index + 2] );
           xi_m1 = Eigen::Vector2d(  parameters[x_index - 2],   parameters[y_index - 2] );
          
           // compute cost
           addSmoothingResidual(  _params.smooth_weight,   xi,   xi_p1,   xi_m1,   cost_raw );
           addCurvatureResidual(  _params.curvature_weight,   xi,   xi_p1,   xi_m1,   curvature_params,   cost_raw );
           addDistanceResidual(  _params.distance_weight,   xi,   _original_path->at(  i ),   cost_raw );
          
           if (  valid_coords = _costmap->worldToMap(  xi[0],   xi[1],   mx,   my ) ) {
           costmap_cost = _costmap->getCost(  mx,   my );
           addCostResidual(  _params.costmap_weight,   costmap_cost,   cost_raw,   xi );
           }
          
           if (  gradient != NULL ) {
           // compute gradient
           gradient[x_index] = 0.0;
           gradient[y_index] = 0.0;
           addSmoothingJacobian(  _params.smooth_weight,   xi,   xi_p1,   xi_m1,   grad_x_raw,   grad_y_raw );
           addCurvatureJacobian(  
           _params.curvature_weight,   xi,   xi_p1,   xi_m1,   curvature_params,  
           grad_x_raw,   grad_y_raw );
           addDistanceJacobian(  
           _params.distance_weight,   xi,   _original_path->at(  
           i ),   grad_x_raw,   grad_y_raw );
          
           if (  valid_coords ) {
           addCostJacobian(  _params.costmap_weight,   mx,   my,   costmap_cost,   grad_x_raw,   grad_y_raw );
           }
          
           gradient[x_index] = grad_x_raw;
           gradient[y_index] = grad_y_raw;
           }
           }
          
           cost[0] = cost_raw;
          
           return true;
           }
          
           /**
           * @brief Get number of parameter blocks
           * @return Number of parameters in cost function
           */
           virtual int NumParameters(   ) const {return _num_params;}
          
          protected:
           /**
           * @brief Cost function term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param r Residual (  cost ) of term
           */
           inline void addSmoothingResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & r ) const
           {
           r += weight * (  
           pt_p.dot(  pt_p ) -
           4 * pt_p.dot(  pt ) +
           2 * pt_p.dot(  pt_m ) +
           4 * pt.dot(  pt ) -
           4 * pt.dot(  pt_m ) +
           pt_m.dot(  pt_m ) ); // objective function value
           }
          
           /**
           * @brief Cost function derivative term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addSmoothingJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & j0,  
           double & j1 ) const
           {
           j0 += weight *
           (  -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
           j1 += weight *
           (  -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
           }
          
           /**
           * @brief Cost function term for maximum curved paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           * @param r Residual (  cost ) of term
           */
           inline void addCurvatureResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           CurvatureComputations & curvature_params,  
           double & r ) const
           {
           curvature_params.valid = true;
           curvature_params.delta_xi = Eigen::Vector2d(  pt[0] - pt_m[0],   pt[1] - pt_m[1] );
           curvature_params.delta_xi_p = Eigen::Vector2d(  pt_p[0] - pt[0],   pt_p[1] - pt[1] );
           curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(   );
           curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(   );
          
           if (  curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
           std::isnan(  curvature_params.delta_xi_p_norm ) || std::isnan(  curvature_params.delta_xi_norm ) ||
           std::isinf(  curvature_params.delta_xi_p_norm ) || std::isinf(  curvature_params.delta_xi_norm ) )
           {
           // ensure we have non-nan values returned
           curvature_params.valid = false;
           return;
           }
          
           const double & delta_xi_by_xi_p =
           curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
           double projection =
           curvature_params.delta_xi.dot(  curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
           if (  fabs(  1 - projection ) < EPSILON || fabs(  projection + 1 ) < EPSILON ) {
           projection = 1.0;
           }
          
           curvature_params.delta_phi_i = std::acos(  projection );
           curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
          
           curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature;
          
           if (  curvature_params.ki_minus_kmax <= EPSILON ) {
           // Quadratic penalty need not apply
           curvature_params.valid = false;
           return;
           }
          
           r += weight *
           curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
           }
          
           /**
           * @brief Cost function derivative term for maximum curvature paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct with cached values to speed up Jacobian computation
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addCurvatureJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & /*pt_m*/,  
           CurvatureComputations & curvature_params,  
           double & j0,  
           double & j1 ) const
           {
           if (  !curvature_params.isValid(   ) ) {
           return;
           }
          
           const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
           -1 / std::sqrt(  1 - std::pow(  std::cos(  curvature_params.delta_phi_i ),   2 ) );
           // const Eigen::Vector2d ones = Eigen::Vector2d(  1.0,   1.0 );
           auto neg_pt_plus = -1 * pt_p;
           Eigen::Vector2d p1 = normalizedOrthogonalComplement(  
           pt,   neg_pt_plus,   curvature_params.delta_xi_norm,   curvature_params.delta_xi_p_norm );
           Eigen::Vector2d p2 = normalizedOrthogonalComplement(  
           neg_pt_plus,   pt,   curvature_params.delta_xi_p_norm,   curvature_params.delta_xi_norm );
          
           const double & u = 2 * curvature_params.ki_minus_kmax;
           const double & common_prefix =
           (  1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
           const double & common_suffix = curvature_params.delta_phi_i /
           (  curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
          
           const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
           curvature_params.delta_xi_norm;
          
           const Eigen::Vector2d jacobian = u *
           (  common_prefix * (  -p1 - p2 ) - (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_im1 = u *
           (  common_prefix * p2 + (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_ip1 = u * (  common_prefix * p1 );
          
           // Old formulation we may require again.
           // j0 += weight *
           // (  jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
           // j1 += weight *
           // (  jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
          
           j0 += weight * jacobian[0]; // xi x component of partial-derivative
           j1 += weight * jacobian[1]; // xi x component of partial-derivative
           }
          
           /**
           * @brief Cost function derivative term for steering away changes in pose
           * @param weight Weight to apply to function
           * @param xi Point Xi for evaluation
           * @param xi_original original point Xi for evaluation
           * @param r Residual (  cost ) of term
           */
           inline void addDistanceResidual(  
           const double & weight,  
           const Eigen::Vector2d & xi,  
           const Eigen::Vector2d & xi_original,  
           double & r ) const
           {
           r += weight * (  xi - xi_original ).dot(  xi - xi_original ); // objective function value
           }
          
           /**
           * @brief Cost function derivative term for steering away changes in pose
           * @param weight Weight to apply to function
           * @param xi Point Xi for evaluation
           * @param xi_original original point Xi for evaluation
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addDistanceJacobian(  
           const double & weight,  
           const Eigen::Vector2d & xi,  
           const Eigen::Vector2d & xi_original,  
           double & j0,  
           double & j1 ) const
           {
           j0 += weight * 2 * (  xi[0] - xi_original[0] ); // xi y component of partial-derivative
           j1 += weight * 2 * (  xi[1] - xi_original[1] ); // xi y component of partial-derivative
           }
          
          
           /**
           * @brief Cost function term for steering away from costs
           * @param weight Weight to apply to function
           * @param value Point Xi's cost'
           * @param params computed values to reduce overhead
           * @param r Residual (  cost ) of term
           */
           inline void addCostResidual(  
           const double & weight,  
           const double & value,  
           double & r,  
           Eigen::Vector2d & xi ) const
           {
           if (  value == FREE ) {
           return;
           }
          
           r += weight * value * value; // objective function value
          
          
           // float obsDst = voronoiDiagram.getDistance(  (  int )xi[0],   (  int )xi[1] );
          
           // if (  abs(  obsDst ) > 0.3 ) {
           // return;
           // }
          
           // r += weight * (  abs(  obsDst ) - 0.3 ) * (  abs(  obsDst ) - 0.3 );
           }
          
           /**
           * @brief Cost function derivative term for steering away from costs
           * @param weight Weight to apply to function
           * @param mx Point Xi's x coordinate in map frame
           * @param mx Point Xi's y coordinate in map frame
           * @param value Point Xi's cost'
           * @param params computed values to reduce overhead
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addCostJacobian(  
           const double & weight,  
           const unsigned int & mx,  
           const unsigned int & my,  
           const double & value,  
           double & j0,  
           double & j1 ) const
           {
           if (  value == FREE ) {
           return;
           }
          
           const Eigen::Vector2d grad = getCostmapGradient(  mx,   my );
           const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value;
          
           j0 += common_prefix * grad[0]; // xi x component of partial-derivative
           j1 += common_prefix * grad[1]; // xi y component of partial-derivative
           }
          
           /**
           * @brief Computing the gradient of the costmap using
           * the 2 point numerical differentiation method
           * @param mx Point Xi's x coordinate in map frame
           * @param mx Point Xi's y coordinate in map frame
           * @param params Params reference to store gradients
           */
           inline Eigen::Vector2d getCostmapGradient(  
           const unsigned int mx,  
           const unsigned int my ) const
           {
           // find unit vector that describes that direction
           // via 7 point taylor series approximation for gradient at Xi
           Eigen::Vector2d gradient;
          
           double l_1 = 0.0;
           double l_2 = 0.0;
           double l_3 = 0.0;
           double r_1 = 0.0;
           double r_2 = 0.0;
           double r_3 = 0.0;
          
           if (  mx < _costmap->getSizeInCellsX(   ) ) {
           r_1 = static_cast<double>(  _costmap->getCost(  mx + 1,   my ) );
           }
           if (  mx + 1 < _costmap->getSizeInCellsX(   ) ) {
           r_2 = static_cast<double>(  _costmap->getCost(  mx + 2,   my ) );
           }
           if (  mx + 2 < _costmap->getSizeInCellsX(   ) ) {
           r_3 = static_cast<double>(  _costmap->getCost(  mx + 3,   my ) );
           }
          
           if (  mx > 0 ) {
           l_1 = static_cast<double>(  _costmap->getCost(  mx - 1,   my ) );
           }
           if (  mx - 1 > 0 ) {
           l_2 = static_cast<double>(  _costmap->getCost(  mx - 2,   my ) );
           }
           if (  mx - 2 > 0 ) {
           l_3 = static_cast<double>(  _costmap->getCost(  mx - 3,   my ) );
           }
          
           gradient[1] = (  45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3 ) / 60;
          
           if (  my < _costmap->getSizeInCellsY(   ) ) {
           r_1 = static_cast<double>(  _costmap->getCost(  mx,   my + 1 ) );
           }
           if (  my + 1 < _costmap->getSizeInCellsY(   ) ) {
           r_2 = static_cast<double>(  _costmap->getCost(  mx,   my + 2 ) );
           }
           if (  my + 2 < _costmap->getSizeInCellsY(   ) ) {
           r_3 = static_cast<double>(  _costmap->getCost(  mx,   my + 3 ) );
           }
          
           if (  my > 0 ) {
           l_1 = static_cast<double>(  _costmap->getCost(  mx,   my - 1 ) );
           }
           if (  my - 1 > 0 ) {
           l_2 = static_cast<double>(  _costmap->getCost(  mx,   my - 2 ) );
           }
           if (  my - 2 > 0 ) {
           l_3 = static_cast<double>(  _costmap->getCost(  mx,   my - 3 ) );
           }
          
           gradient[0] = (  45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3 ) / 60;
          
           gradient.normalize(   );
           return gradient;
           }
          
           /**
           * @brief Computing the normalized orthogonal component of 2 vectors
           * @param a Vector
           * @param b Vector
           * @param norm a Vector's norm
           * @param norm b Vector's norm
           * @return Normalized vector of orthogonal components
           */
           inline Eigen::Vector2d normalizedOrthogonalComplement(  
           const Eigen::Vector2d & a,  
           const Eigen::Vector2d & b,  
           const double & a_norm,  
           const double & b_norm ) const
           {
           return (  a - (  a.dot(  b ) * b / b.squaredNorm(   ) ) ) / (  a_norm * b_norm );
           }
          
           std::vector<Eigen::Vector2d> * _original_path{nullptr};
           int _num_params;
           nav2_costmap_2d::Costmap2D * _costmap{nullptr};
           SmootherParams _params;
           // DynamicVoronoi voronoiDiagram;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler.hpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef DEPRECATED__UPSAMPLER_HPP_
          #define DEPRECATED__UPSAMPLER_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <memory>
          #include <queue>
          #include <algorithm>
          #include <utility>
          
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/upsampler_cost_function.hpp"
          #include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          
          namespace nav2_smac_planner
          {
          
          /**
           * @class nav2_smac_planner::Upsampler
           * @brief A Conjugate Gradient 2D path upsampler implementation
           */
      40  class Upsampler
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::Upsampler
           */
      46   Upsampler(   ) {}
          
           /**
           * @brief A destructor for nav2_smac_planner::Upsampler
           */
      51   ~Upsampler(   ) {}
          
           /**
           * @brief Initialization of the Upsampler
           */
      56   void initialize(  const OptimizerParams params )
           {
           _debug = params.debug;
          
           // General Params
          
           // 2 most valid options: STEEPEST_DESCENT,   NONLINEAR_CONJUGATE_GRADIENT
           _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
           _options.line_search_type = ceres::WOLFE;
           _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
           _options.line_search_interpolation_type = ceres::CUBIC;
          
           _options.max_num_iterations = params.max_iterations; // 5000
           _options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO
          
           _options.function_tolerance = params.fn_tol;
           _options.gradient_tolerance = params.gradient_tol;
           _options.parameter_tolerance = params.param_tol; // 1e-20;
          
           _options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30;
           _options.max_num_line_search_step_size_iterations =
           params.advanced.max_num_line_search_step_size_iterations;
           _options.line_search_sufficient_function_decrease =
           params.advanced.line_search_sufficient_function_decrease; // 1e-30;
           _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
           _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
           _options.max_num_line_search_direction_restarts =
           params.advanced.max_num_line_search_direction_restarts;
           _options.line_search_sufficient_curvature_decrease =
           params.advanced.line_search_sufficient_curvature_decrease;
           _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
          
           if (  _debug ) {
           _options.minimizer_progress_to_stdout = true;
           } else {
           _options.logging_type = ceres::SILENT;
           }
           }
          
           /**
           * @brief Upsampling method
           * @param path Reference to path
           * @param upsample parameters weights
           * @param upsample_ratio upsample ratio
           * @return If Upsampler was successful
           */
     102   bool upsample(  
     103   std::vector<Eigen::Vector2d> & path,  
           const SmootherParams & params,  
           const int & upsample_ratio )
           {
           _options.max_solver_time_in_seconds = params.max_time;
          
           if (  upsample_ratio != 2 && upsample_ratio != 4 ) {
           // invalid inputs
           return false;
           }
          
           const int param_ratio = upsample_ratio * 2.0;
           const int total_size = 2 * (  path.size(   ) * upsample_ratio - upsample_ratio + 1 );
           double parameters[total_size]; // NOLINT
          
           // 20-4hz regularly,   but dosnt work in faster cases
           // Linearly distribute initial poses for optimization
           // TODO(  stevemacenski ) generalize for 2x and 4x
           unsigned int next_pt;
           Eigen::Vector2d interpolated;
           std::vector<Eigen::Vector2d> temp_path;
           for (  unsigned int pt = 0; pt != path.size(   ) - 1; pt++ ) {
           next_pt = pt + 1;
           interpolated = (  path[next_pt] + path[pt] ) / 2.0;
          
           parameters[param_ratio * pt] = path[pt][0];
           parameters[param_ratio * pt + 1] = path[pt][1];
           temp_path.push_back(  path[pt] );
          
           parameters[param_ratio * pt + 2] = interpolated[0];
           parameters[param_ratio * pt + 3] = interpolated[1];
           temp_path.push_back(  interpolated );
           }
          
           parameters[total_size - 2] = path.back(   )[0];
           parameters[total_size - 1] = path.back(   )[1];
           temp_path.push_back(  path.back(   ) );
          
           // Solve the upsampling problem
           ceres::GradientProblemSolver::Summary summary;
           ceres::GradientProblem problem(  new UpsamplerCostFunction(  temp_path,   params,   upsample_ratio ) );
           ceres::Solve(  _options,   problem,   parameters,   &summary );
          
          
           path.resize(  total_size / 2 );
           for (  int i = 0; i != total_size / 2; i++ ) {
           path[i][0] = parameters[2 * i];
           path[i][1] = parameters[2 * i + 1];
           }
          
           // 10-15 hz,   regularly
           // std::vector<Eigen::Vector2d> path_double_sampled;
           // for (  int i = 0; i != path.size(   ) - 1; i++ ) { // last term should not be upsampled
           // path_double_sampled.push_back(  path[i] );
           // path_double_sampled.push_back(  (  path[i+1] + path[i] ) / 2 );
           // }
          
           // std::unique_ptr<ceres::Problem> problem = std::make_unique<ceres::Problem>(   );
           // for (  uint i = 1; i != path_double_sampled.size(   ) - 1; i++ ) {
           // ceres::CostFunction * cost_fn =
           // new UpsamplerConstrainedCostFunction(  path_double_sampled,   params,   2,   i );
           // problem->AddResidualBlock(  
           // cost_fn,   nullptr,   &path_double_sampled[i][0],   &path_double_sampled[i][1] );
           // // locking initial coordinates unnecessary since there's no update between terms in NLLS
           // }
          
           // ceres::Solver::Summary summary;
           // _options.minimizer_type = ceres::LINE_SEARCH;
           // ceres::Solve(  _options,   problem.get(   ),   &summary );
          
           // if (  upsample_ratio == 4 ) {
           // std::vector<Eigen::Vector2d> path_quad_sampled;
           // for (  int i = 0; i != path_double_sampled.size(   ) - 1; i++ ) {
           // path_quad_sampled.push_back(  path_double_sampled[i] );
           // path_quad_sampled.push_back(  (  path_double_sampled[i+1] + path_double_sampled[i] ) / 2.0 );
           // }
          
           // std::unique_ptr<ceres::Problem> problem2 = std::make_unique<ceres::Problem>(   );
           // for (  uint i = 1; i != path_quad_sampled.size(   ) - 1; i++ ) {
           // ceres::CostFunction * cost_fn =
           // new UpsamplerConstrainedCostFunction(  path_quad_sampled,   params,   4,   i );
           // problem2->AddResidualBlock(  
           // cost_fn,   nullptr,   &path_quad_sampled[i][0],   &path_quad_sampled[i][1] );
           // }
          
           // ceres::Solve(  _options,   problem2.get(   ),   &summary );
          
           // path = path_quad_sampled;
           // } else {
           // path = path_double_sampled;
           // }
          
           if (  _debug ) {
           std::cout << summary.FullReport(   ) << '\n';
           }
          
           if (  !summary.IsSolutionUsable(   ) || summary.initial_cost - summary.final_cost <= 0.0 ) {
           return false;
           }
          
           return true;
           }
          
          private:
     207   bool _debug;
     208   ceres::GradientProblemSolver::Options _options;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // DEPRECATED__UPSAMPLER_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
          #define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <unordered_map>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/options.hpp"
          
          #define EPSILON 0.0001
          
          namespace nav2_smac_planner
          {
          /**
           * @struct nav2_smac_planner::UpsamplerCostFunction
           * @brief Cost function for path upsampling with multiple terms using unconstrained
           * optimization including curvature,   smoothness,   collision,   and avoid obstacles.
           */
      40  class UpsamplerCostFunction : public ceres::FirstOrderFunction
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
           * @param num_points Number of path points to consider
           */
      47   UpsamplerCostFunction(  
      48   const std::vector<Eigen::Vector2d> & path,  
           const SmootherParams & params,  
           const int & upsample_ratio )
           : _num_params(  2 * path.size(   ) ),  
           _params(  params ),  
           _upsample_ratio(  upsample_ratio ),  
           _path(  path )
           {
           }
           // TODO(  stevemacenski ) removed upsample_ratio because temp upsampling on path size
          
           /**
           * @struct CurvatureComputations
           * @brief Cache common computations between the curvature terms to minimize recomputations
           */
           struct CurvatureComputations
           {
           /**
           * @brief A constructor for nav2_smac_planner::CurvatureComputations
           */
           CurvatureComputations(   )
           {
           valid = false;
           }
          
           bool valid;
           /**
           * @brief Check if result is valid for penalty
           * @return is valid (  non-nan,   non-inf,   and turning angle > max )
           */
      78   bool isValid(   )
           {
           return valid;
           }
          
           Eigen::Vector2d delta_xi{0,   0};
           Eigen::Vector2d delta_xi_p{0,   0};
           double delta_xi_norm{0};
           double delta_xi_p_norm{0};
           double delta_phi_i{0};
           double turning_rad{0};
           double ki_minus_kmax{0};
           };
          
           /**
           * @brief Smoother cost function evaluation
           * @param parameters X,  Y pairs of points
           * @param cost total cost of path
           * @param gradient of path at each X,  Y pair from cost function derived analytically
           * @return if successful in computing values
           */
           virtual bool Evaluate(  
           const double * parameters,  
           double * cost,  
           double * gradient ) const
           {
           Eigen::Vector2d xi;
           Eigen::Vector2d xi_p1;
           Eigen::Vector2d xi_m1;
           uint x_index,   y_index;
           cost[0] = 0.0;
           double cost_raw = 0.0;
           double grad_x_raw = 0.0;
           double grad_y_raw = 0.0;
          
           // cache some computations between the residual and jacobian
           CurvatureComputations curvature_params;
          
           for (  int i = 0; i != NumParameters(   ) / 2; i++ ) {
           x_index = 2 * i;
           y_index = 2 * i + 1;
           gradient[x_index] = 0.0;
           gradient[y_index] = 0.0;
           if (  i < 1 || i >= NumParameters(   ) / 2 - 1 ) {
           continue;
           }
          
           // if original point's neighbors TODO
           if (  i % _upsample_ratio == 1 ) {
           continue;
           }
          
           xi = Eigen::Vector2d(  parameters[x_index],   parameters[y_index] );
          
           // TODO(  stevemacenski ): from deep copy to make sure no feedback _path
           xi_p1 = _path.at(  i + 1 );
           xi_m1 = _path.at(  i - 1 );
           // xi_p1 = Eigen::Vector2d(  parameters[x_index + 2],   parameters[y_index + 2] );
           // xi_m1 = Eigen::Vector2d(  parameters[x_index - 2],   parameters[y_index - 2] );
          
           // compute cost
           addSmoothingResidual(  15000,   xi,   xi_p1,   xi_m1,   cost_raw );
           addCurvatureResidual(  60.0,   xi,   xi_p1,   xi_m1,   curvature_params,   cost_raw );
          
           if (  gradient != NULL ) {
           // compute gradient
           addSmoothingJacobian(  15000,   xi,   xi_p1,   xi_m1,   grad_x_raw,   grad_y_raw );
           addCurvatureJacobian(  60.0,   xi,   xi_p1,   xi_m1,   curvature_params,   grad_x_raw,   grad_y_raw );
          
           gradient[x_index] = grad_x_raw;
           gradient[y_index] = grad_y_raw;
           }
           }
          
           cost[0] = cost_raw;
           return true;
           }
          
           /**
           * @brief Get number of parameter blocks
           * @return Number of parameters in cost function
           */
           virtual int NumParameters(   ) const {return _num_params;}
          
          protected:
           /**
           * @brief Cost function term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param r Residual (  cost ) of term
           */
           inline void addSmoothingResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & r ) const
           {
           r += weight * (  
           pt_p.dot(  pt_p ) -
           4 * pt_p.dot(  pt ) +
           2 * pt_p.dot(  pt_m ) +
           4 * pt.dot(  pt ) -
           4 * pt.dot(  pt_m ) +
           pt_m.dot(  pt_m ) ); // objective function value
           }
          
           /**
           * @brief Cost function derivative term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addSmoothingJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & j0,  
           double & j1 ) const
           {
           j0 += weight *
           (  -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
           j1 += weight *
           (  -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
           }
          
           /**
           * @brief Get path curvature information
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           */
           inline void getCurvatureParams(  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           CurvatureComputations & curvature_params ) const
           {
           curvature_params.valid = true;
           curvature_params.delta_xi = Eigen::Vector2d(  pt[0] - pt_m[0],   pt[1] - pt_m[1] );
           curvature_params.delta_xi_p = Eigen::Vector2d(  pt_p[0] - pt[0],   pt_p[1] - pt[1] );
           curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(   );
           curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(   );
          
           if (  curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
           std::isnan(  curvature_params.delta_xi_p_norm ) || std::isnan(  curvature_params.delta_xi_norm ) ||
           std::isinf(  curvature_params.delta_xi_p_norm ) || std::isinf(  curvature_params.delta_xi_norm ) )
           {
           // ensure we have non-nan values returned
           curvature_params.valid = false;
           return;
           }
          
           const double & delta_xi_by_xi_p =
           curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
           double projection =
           curvature_params.delta_xi.dot(  curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
           if (  fabs(  1 - projection ) < EPSILON || fabs(  projection + 1 ) < EPSILON ) {
           projection = 1.0;
           }
          
           curvature_params.delta_phi_i = std::acos(  projection );
           curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
          
           curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
           _params.max_curvature;
           // TODO(  stevemacenski ) is use of upsample_ratio correct here? small number?
           // TODO(  stevemacenski ) can remove the subtraction with a
           // lower weight value,   does have direction issue,   maybe just tuning?
          
           if (  curvature_params.ki_minus_kmax <= EPSILON ) {
           // Quadratic penalty need not apply
           curvature_params.valid = false;
           }
           }
          
           /**
           * @brief Cost function term for maximum curved paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           * @param r Residual (  cost ) of term
           */
           inline void addCurvatureResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           CurvatureComputations & curvature_params,  
           double & r ) const
           {
           getCurvatureParams(  pt,   pt_p,   pt_m,   curvature_params );
          
           if (  !curvature_params.isValid(   ) ) {
           return;
           }
          
           r += weight *
           curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
           }
          
           /**
           * @brief Cost function derivative term for maximum curvature paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct with cached values to speed up Jacobian computation
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addCurvatureJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & /*pt_m*/,  
           CurvatureComputations & curvature_params,  
           double & j0,  
           double & j1 ) const
           {
           if (  !curvature_params.isValid(   ) ) {
           return;
           }
          
           const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
           -1 / std::sqrt(  1 - std::pow(  std::cos(  curvature_params.delta_phi_i ),   2 ) );
           // const Eigen::Vector2d ones = Eigen::Vector2d(  1.0,   1.0 );
           auto neg_pt_plus = -1 * pt_p;
           Eigen::Vector2d p1 = normalizedOrthogonalComplement(  
           pt,   neg_pt_plus,   curvature_params.delta_xi_norm,   curvature_params.delta_xi_p_norm );
           Eigen::Vector2d p2 = normalizedOrthogonalComplement(  
           neg_pt_plus,   pt,   curvature_params.delta_xi_p_norm,   curvature_params.delta_xi_norm );
          
           const double & u = 2 * curvature_params.ki_minus_kmax;
           const double & common_prefix =
           (  1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
           const double & common_suffix = curvature_params.delta_phi_i /
           (  curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
           const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
           curvature_params.delta_xi_norm;
          
           const Eigen::Vector2d jacobian = u *
           (  common_prefix * (  -p1 - p2 ) - (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_im1 = u *
           (  common_prefix * p2 + (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_ip1 = u * (  common_prefix * p1 );
           j0 += weight * jacobian[0]; // xi y component of partial-derivative
           j1 += weight * jacobian[1]; // xi x component of partial-derivative
           // j0 += weight *
           // (  jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
           // j1 += weight *
           // (  jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
           }
          
           /**
           * @brief Computing the normalized orthogonal component of 2 vectors
           * @param a Vector
           * @param b Vector
           * @param norm a Vector's norm
           * @param norm b Vector's norm
           * @return Normalized vector of orthogonal components
           */
           inline Eigen::Vector2d normalizedOrthogonalComplement(  
           const Eigen::Vector2d & a,  
           const Eigen::Vector2d & b,  
           const double & a_norm,  
           const double & b_norm ) const
           {
           return (  a - (  a.dot(  b ) * b / b.squaredNorm(   ) ) ) / (  a_norm * b_norm );
           }
          
           int _num_params;
           SmootherParams _params;
           int _upsample_ratio;
           std::vector<Eigen::Vector2d> _path;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp

          // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
          #define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
          
          #include <cmath>
          #include <vector>
          #include <iostream>
          #include <unordered_map>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "ceres/ceres.h"
          #include "Eigen/Core"
          #include "nav2_smac_planner/types.hpp"
          #include "nav2_smac_planner/options.hpp"
          
          #define EPSILON 0.0001
          
          namespace nav2_smac_planner
          {
          /**
           * @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
           * @brief Cost function for path upsampling with multiple terms using NLLS
           * including curvature,   smoothness,   collision,   and avoid obstacles.
           */
      40  class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1,   1,   1>
          {
          public:
           /**
           * @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
           * @param num_points Number of path points to consider
           */
      47   UpsamplerConstrainedCostFunction(  
      48   const std::vector<Eigen::Vector2d> & path,  
           const SmootherParams & params,  
           const int & upsample_ratio,  
           const int & i )
           : _path(  path ),  
           _params(  params ),  
           _upsample_ratio(  upsample_ratio ),  
           index(  i )
           {
           }
          
           /**
           * @struct CurvatureComputations
           * @brief Cache common computations between the curvature terms to minimize recomputations
           */
           struct CurvatureComputations
           {
           /**
           * @brief A constructor for nav2_smac_planner::CurvatureComputations
           */
           CurvatureComputations(   )
           {
           valid = true;
           }
          
           bool valid;
           /**
           * @brief Check if result is valid for penalty
           * @return is valid (  non-nan,   non-inf,   and turning angle > max )
           */
      78   bool isValid(   )
           {
           return valid;
           }
          
           Eigen::Vector2d delta_xi{0,   0};
           Eigen::Vector2d delta_xi_p{0,   0};
           double delta_xi_norm{0};
           double delta_xi_p_norm{0};
           double delta_phi_i{0};
           double turning_rad{0};
           double ki_minus_kmax{0};
           };
          
           /**
           * @brief Smoother cost function evaluation
           * @param parameters X,  Y pairs of points
           * @param cost total cost of path
           * @param gradient of path at each X,  Y pair from cost function derived analytically
           * @return if successful in computing values
           */
          
           bool Evaluate(  
           double const * const * parameters,  
           double * residuals,  
           double ** jacobians ) const override
           {
           Eigen::Vector2d xi = Eigen::Vector2d(  parameters[0][0],   parameters[1][0] );
           Eigen::Vector2d xi_p1 = _path.at(  index + 1 );
           Eigen::Vector2d xi_m1 = _path.at(  index - 1 );
           CurvatureComputations curvature_params;
           double grad_x_raw = 0,   grad_y_raw = 0,   cost_raw = 0;
          
           // compute cost
           addSmoothingResidual(  15000,   xi,   xi_p1,   xi_m1,   cost_raw );
           addCurvatureResidual(  60.0,   xi,   xi_p1,   xi_m1,   curvature_params,   cost_raw );
          
           residuals[0] = 0;
           residuals[0] = cost_raw; // objective function value x
          
           if (  jacobians != NULL && jacobians[0] != NULL ) {
           addSmoothingJacobian(  15000,   xi,   xi_p1,   xi_m1,   grad_x_raw,   grad_y_raw );
           addCurvatureJacobian(  60.0,   xi,   xi_p1,   xi_m1,   curvature_params,   grad_x_raw,   grad_y_raw );
          
           jacobians[0][0] = 0;
           jacobians[1][0] = 0;
           jacobians[0][0] = grad_x_raw; // x derivative
           jacobians[1][0] = grad_y_raw; // y derivative
           jacobians[0][1] = 0.0;
           jacobians[1][1] = 0.0;
           }
          
           return true;
           }
          
          protected:
           /**
           * @brief Cost function term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param r Residual (  cost ) of term
           */
           inline void addSmoothingResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & r ) const
           {
           r += weight * (  
           pt_p.dot(  pt_p ) -
           4 * pt_p.dot(  pt ) +
           2 * pt_p.dot(  pt_m ) +
           4 * pt.dot(  pt ) -
           4 * pt.dot(  pt_m ) +
           pt_m.dot(  pt_m ) ); // objective function value
           }
          
           /**
           * @brief Cost function derivative term for smooth paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addSmoothingJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           double & j0,  
           double & j1 ) const
           {
           j0 += weight *
           (  -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
           j1 += weight *
           (  -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
           }
          
           /**
           * @brief Get path curvature information
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           */
           inline void getCurvatureParams(  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           CurvatureComputations & curvature_params ) const
           {
           curvature_params.valid = true;
           curvature_params.delta_xi = Eigen::Vector2d(  pt[0] - pt_m[0],   pt[1] - pt_m[1] );
           curvature_params.delta_xi_p = Eigen::Vector2d(  pt_p[0] - pt[0],   pt_p[1] - pt[1] );
           curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(   );
           curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(   );
          
           if (  curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
           std::isnan(  curvature_params.delta_xi_p_norm ) || std::isnan(  curvature_params.delta_xi_norm ) ||
           std::isinf(  curvature_params.delta_xi_p_norm ) || std::isinf(  curvature_params.delta_xi_norm ) )
           {
           // ensure we have non-nan values returned
           curvature_params.valid = false;
           return;
           }
          
           const double & delta_xi_by_xi_p =
           curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
           double projection =
           curvature_params.delta_xi.dot(  curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
           if (  fabs(  1 - projection ) < EPSILON || fabs(  projection + 1 ) < EPSILON ) {
           projection = 1.0;
           }
          
           curvature_params.delta_phi_i = std::acos(  projection );
           curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
          
           curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
           _params.max_curvature;
          
           if (  curvature_params.ki_minus_kmax <= EPSILON ) {
           // Quadratic penalty need not apply
           curvature_params.valid = false;
           }
           }
          
           /**
           * @brief Cost function term for maximum curved paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct to cache computations for the jacobian to use
           * @param r Residual (  cost ) of term
           */
           inline void addCurvatureResidual(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & pt_m,  
           CurvatureComputations & curvature_params,  
           double & r ) const
           {
           getCurvatureParams(  pt,   pt_p,   pt_m,   curvature_params );
          
           if (  !curvature_params.isValid(   ) ) {
           return;
           }
          
           // objective function value
           r += weight *
           curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;
           }
          
           /**
           * @brief Cost function derivative term for maximum curvature paths
           * @param weight Weight to apply to function
           * @param pt Point Xi for evaluation
           * @param pt Point Xi+1 for calculating Xi's cost
           * @param pt Point Xi-1 for calculating Xi's cost
           * @param curvature_params A struct with cached values to speed up Jacobian computation
           * @param j0 Gradient of X term
           * @param j1 Gradient of Y term
           */
           inline void addCurvatureJacobian(  
           const double & weight,  
           const Eigen::Vector2d & pt,  
           const Eigen::Vector2d & pt_p,  
           const Eigen::Vector2d & /*pt_m*/,  
           CurvatureComputations & curvature_params,  
           double & j0,  
           double & j1 ) const
           {
           if (  !curvature_params.isValid(   ) ) {
           return;
           }
          
           const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
           -1 / std::sqrt(  1 - std::pow(  std::cos(  curvature_params.delta_phi_i ),   2 ) );
           // const Eigen::Vector2d ones = Eigen::Vector2d(  1.0,   1.0 );
           auto neg_pt_plus = -1 * pt_p;
           Eigen::Vector2d p1 = normalizedOrthogonalComplement(  
           pt,   neg_pt_plus,   curvature_params.delta_xi_norm,   curvature_params.delta_xi_p_norm );
           Eigen::Vector2d p2 = normalizedOrthogonalComplement(  
           neg_pt_plus,   pt,   curvature_params.delta_xi_p_norm,   curvature_params.delta_xi_norm );
          
           const double & u = 2 * curvature_params.ki_minus_kmax;
           const double & common_prefix =
           (  1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
           const double & common_suffix = curvature_params.delta_phi_i /
           (  curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
           const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
           curvature_params.delta_xi_norm;
          
           const Eigen::Vector2d jacobian = u *
           (  common_prefix * (  -p1 - p2 ) - (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_im1 = u *
           (  common_prefix * p2 + (  common_suffix * d_delta_xi_d_xi ) );
           const Eigen::Vector2d jacobian_ip1 = u * (  common_prefix * p1 );
           j0 += weight * jacobian[0]; // xi x component of partial-derivative
           j1 += weight * jacobian[1]; // xi y component of partial-derivative
           // j0 += weight *
           // (  jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
           // j1 += weight *
           // (  jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
           }
           /**
           * @brief Computing the normalized orthogonal component of 2 vectors
           * @param a Vector
           * @param b Vector
           * @param norm a Vector's norm
           * @param norm b Vector's norm
           * @return Normalized vector of orthogonal components
           */
           inline Eigen::Vector2d normalizedOrthogonalComplement(  
           const Eigen::Vector2d & a,  
           const Eigen::Vector2d & b,  
           const double & a_norm,  
           const double & b_norm ) const
           {
           return (  a - (  a.dot(  b ) * b / b.squaredNorm(   ) ) ) / (  a_norm * b_norm );
           }
          
           std::vector<Eigen::Vector2d> _path;
           SmootherParams _params;
           int _upsample_ratio;
           int index;
          };
          
          } // namespace nav2_smac_planner
          
          #endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_

navigation2/nav2_smac_planner/test/test_a_star.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
      32  class RclCppFixture
          {
          public:
      35   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      36   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      40  TEST(  AStarTest,   test_a_star_2d )
          {
           nav2_smac_planner::SearchInfo info;
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(  
           nav2_smac_planner::MotionModel::MOORE,   info );
           int max_iterations = 10000;
           float tolerance = 0.0;
           float some_tolerance = 20.0;
           int it_on_approach = 10;
           double max_planning_time = 120.0;
           int num_it = 0;
          
           a_star.initialize(  false,   max_iterations,   it_on_approach,   max_planning_time,   0.0,   1 );
          
           nav2_costmap_2d::Costmap2D * costmapA =
           new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0.0,   0.0,   0 );
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 40; i <= 60; ++i ) {
           for (  unsigned int j = 40; j <= 60; ++j ) {
           costmapA->setCost(  i,   j,   254 );
           }
           }
          
           // functional case testing
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   1 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
           a_star.setCollisionChecker(  checker.get(   ) );
           a_star.setStart(  20u,   20u,   0 );
           a_star.setGoal(  80u,   80u,   0 );
           nav2_smac_planner::Node2D::CoordinateVector path;
           EXPECT_TRUE(  a_star.createPath(  path,   num_it,   tolerance ) );
           EXPECT_EQ(  num_it,   102 );
          
           // check path is the right size and collision free
           EXPECT_EQ(  path.size(   ),   81u );
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           EXPECT_EQ(  costmapA->getCost(  path[i].x,   path[i].y ),   0 );
           }
          
           // setting non-zero dim 3 for 2D search
           EXPECT_THROW(  a_star.setGoal(  0,   0,   10 ),   std::runtime_error );
           EXPECT_THROW(  a_star.setStart(  0,   0,   10 ),   std::runtime_error );
          
           path.clear(   );
           // failure cases with invalid inputs
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(  
           nav2_smac_planner::MotionModel::VON_NEUMANN,   info );
           a_star_2.initialize(  false,   max_iterations,   it_on_approach,   max_planning_time,   0,   1 );
           num_it = 0;
           EXPECT_THROW(  a_star_2.createPath(  path,   num_it,   tolerance ),   std::runtime_error );
           a_star_2.setCollisionChecker(  checker.get(   ) );
           num_it = 0;
           EXPECT_THROW(  a_star_2.createPath(  path,   num_it,   tolerance ),   std::runtime_error );
           a_star_2.setStart(  50,   50,   0 ); // invalid
           a_star_2.setGoal(  0,   0,   0 ); // valid
           num_it = 0;
           EXPECT_THROW(  a_star_2.createPath(  path,   num_it,   tolerance ),   std::runtime_error );
           a_star_2.setStart(  0,   0,   0 ); // valid
           a_star_2.setGoal(  50,   50,   0 ); // invalid
           num_it = 0;
           EXPECT_THROW(  a_star_2.createPath(  path,   num_it,   tolerance ),   std::runtime_error );
           num_it = 0;
           // invalid goal but liberal tolerance
           a_star_2.setStart(  20,   20,   0 ); // valid
           a_star_2.setGoal(  50,   50,   0 ); // invalid
           EXPECT_TRUE(  a_star_2.createPath(  path,   num_it,   some_tolerance ) );
           EXPECT_EQ(  path.size(   ),   42u );
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           EXPECT_EQ(  costmapA->getCost(  path[i].x,   path[i].y ),   0 );
           }
          
           EXPECT_TRUE(  a_star_2.getStart(   ) != nullptr );
           EXPECT_TRUE(  a_star_2.getGoal(   ) != nullptr );
           EXPECT_EQ(  a_star_2.getSizeX(   ),   100u );
           EXPECT_EQ(  a_star_2.getSizeY(   ),   100u );
           EXPECT_EQ(  a_star_2.getSizeDim3(   ),   1u );
           EXPECT_EQ(  a_star_2.getToleranceHeuristic(   ),   20.0 );
           EXPECT_EQ(  a_star_2.getOnApproachMaxIterations(   ),   10 );
          
           delete costmapA;
          }
          
     123  TEST(  AStarTest,   test_a_star_se2 )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.1;
           info.non_straight_penalty = 1.1;
           info.reverse_penalty = 2.0;
           info.minimum_turning_radius = 8; // in grid coordinates
           info.retrospective_penalty = 0.015;
           info.analytic_expansion_max_length = 20.0; // in grid coordinates
           info.analytic_expansion_ratio = 3.5;
           unsigned int size_theta = 72;
           info.cost_penalty = 1.7;
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(  
           nav2_smac_planner::MotionModel::DUBIN,   info );
           int max_iterations = 10000;
           float tolerance = 10.0;
           int it_on_approach = 10;
           double max_planning_time = 120.0;
           int num_it = 0;
          
           a_star.initialize(  false,   max_iterations,   it_on_approach,   max_planning_time,   401,   size_theta );
          
           nav2_costmap_2d::Costmap2D * costmapA =
           new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0.0,   0.0,   0 );
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 40; i <= 60; ++i ) {
           for (  unsigned int j = 40; j <= 60; ++j ) {
           costmapA->setCost(  i,   j,   254 );
           }
           }
          
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   size_theta );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // functional case testing
           a_star.setCollisionChecker(  checker.get(   ) );
           a_star.setStart(  10u,   10u,   0u );
           a_star.setGoal(  80u,   80u,   40u );
           nav2_smac_planner::NodeHybrid::CoordinateVector path;
           EXPECT_TRUE(  a_star.createPath(  path,   num_it,   tolerance ) );
          
           // check path is the right size and collision free
           EXPECT_EQ(  num_it,   3222 );
           EXPECT_EQ(  path.size(   ),   62u );
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           EXPECT_EQ(  costmapA->getCost(  path[i].x,   path[i].y ),   0 );
           }
           // no skipped nodes
           for (  unsigned int i = 1; i != path.size(   ); i++ ) {
           EXPECT_LT(  hypotf(  path[i].x - path[i - 1].x,   path[i].y - path[i - 1].y ),   2.1f );
           }
          
           delete costmapA;
          }
          
     179  TEST(  AStarTest,   test_a_star_lattice )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.05;
           info.non_straight_penalty = 1.05;
           info.reverse_penalty = 2.0;
           info.retrospective_penalty = 0.1;
           info.analytic_expansion_ratio = 3.5;
           info.lattice_filepath =
           ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" ) +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
           info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
           info.analytic_expansion_max_length = 20.0; // in grid coordinates
           unsigned int size_theta = 16;
           info.cost_penalty = 2.0;
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeLattice> a_star(  
           nav2_smac_planner::MotionModel::STATE_LATTICE,   info );
           int max_iterations = 10000;
           float tolerance = 10.0;
           int it_on_approach = 10;
           double max_planning_time = 120.0;
           int num_it = 0;
          
           a_star.initialize(  
           false,   max_iterations,   std::numeric_limits<int>::max(   ),   max_planning_time,   401,   size_theta );
          
           nav2_costmap_2d::Costmap2D * costmapA =
           new nav2_costmap_2d::Costmap2D(  100,   100,   0.05,   0.0,   0.0,   0 );
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 20; i <= 30; ++i ) {
           for (  unsigned int j = 20; j <= 30; ++j ) {
           costmapA->setCost(  i,   j,   254 );
           }
           }
          
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   size_theta );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // functional case testing
           a_star.setCollisionChecker(  checker.get(   ) );
           a_star.setStart(  5u,   5u,   0u );
           a_star.setGoal(  40u,   40u,   1u );
           nav2_smac_planner::NodeLattice::CoordinateVector path;
           EXPECT_TRUE(  a_star.createPath(  path,   num_it,   tolerance ) );
          
           // check path is the right size and collision free
           EXPECT_EQ(  num_it,   21 );
           EXPECT_EQ(  path.size(   ),   48u );
           for (  unsigned int i = 0; i != path.size(   ); i++ ) {
           EXPECT_EQ(  costmapA->getCost(  path[i].x,   path[i].y ),   0 );
           }
           // no skipped nodes
           for (  unsigned int i = 1; i != path.size(   ); i++ ) {
           EXPECT_LT(  hypotf(  path[i].x - path[i - 1].x,   path[i].y - path[i - 1].y ),   2.1f );
           }
          
           delete costmapA;
          }
          
     240  TEST(  AStarTest,   test_se2_single_pose_path )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.1;
           info.non_straight_penalty = 1.1;
           info.reverse_penalty = 2.0;
           info.retrospective_penalty = 0.0;
           info.minimum_turning_radius = 8; // in grid coordinates
           info.analytic_expansion_max_length = 20.0; // in grid coordinates
           info.analytic_expansion_ratio = 3.5;
           unsigned int size_theta = 72;
           info.cost_penalty = 1.7;
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(  
           nav2_smac_planner::MotionModel::DUBIN,   info );
           int max_iterations = 100;
           float tolerance = 10.0;
           int it_on_approach = 10;
           double max_planning_time = 120.0;
           int num_it = 0;
          
           a_star.initialize(  false,   max_iterations,   it_on_approach,   max_planning_time,   401,   size_theta );
          
           nav2_costmap_2d::Costmap2D * costmapA =
           new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0.0,   0.0,   0 );
          
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   size_theta );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // functional case testing
           a_star.setCollisionChecker(  checker.get(   ) );
           a_star.setStart(  10u,   10u,   0u );
           // Goal is one costmap cell away
           a_star.setGoal(  12u,   10u,   0u );
           nav2_smac_planner::NodeHybrid::CoordinateVector path;
           EXPECT_TRUE(  a_star.createPath(  path,   num_it,   tolerance ) );
          
           // Check that the path is length one
           // With the current implementation,   this produces a longer path
           // EXPECT_EQ(  path.size(   ),   1u );
           EXPECT_GE(  path.size(   ),   1u );
          
           delete costmapA;
          }
          
     285  TEST(  AStarTest,   test_constants )
          {
           nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown
           EXPECT_EQ(  nav2_smac_planner::toString(  mm ),   std::string(  "Unknown" ) );
           mm = nav2_smac_planner::MotionModel::VON_NEUMANN; // vonneumann
           EXPECT_EQ(  nav2_smac_planner::toString(  mm ),   std::string(  "Von Neumann" ) );
           mm = nav2_smac_planner::MotionModel::MOORE; // moore
           EXPECT_EQ(  nav2_smac_planner::toString(  mm ),   std::string(  "Moore" ) );
           mm = nav2_smac_planner::MotionModel::DUBIN; // dubin
           EXPECT_EQ(  nav2_smac_planner::toString(  mm ),   std::string(  "Dubin" ) );
           mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp
           EXPECT_EQ(  nav2_smac_planner::toString(  mm ),   std::string(  "Reeds-Shepp" ) );
          
           EXPECT_EQ(  
           nav2_smac_planner::fromString(  
           "VON_NEUMANN" ),   nav2_smac_planner::MotionModel::VON_NEUMANN );
           EXPECT_EQ(  nav2_smac_planner::fromString(  "MOORE" ),   nav2_smac_planner::MotionModel::MOORE );
           EXPECT_EQ(  nav2_smac_planner::fromString(  "DUBIN" ),   nav2_smac_planner::MotionModel::DUBIN );
           EXPECT_EQ(  
           nav2_smac_planner::fromString(  
           "REEDS_SHEPP" ),   nav2_smac_planner::MotionModel::REEDS_SHEPP );
           EXPECT_EQ(  nav2_smac_planner::fromString(  "NONE" ),   nav2_smac_planner::MotionModel::UNKNOWN );
          }

navigation2/nav2_smac_planner/test/test_collision_checker.cpp

       1  // Copyright (  c ) 2020 Shivang Patel
          // Copyright (  c ) 2020 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <vector>
          #include <memory>
          
          #include "gtest/gtest.h"
          #include "nav2_smac_planner/collision_checker.hpp"
          
          using namespace nav2_costmap_2d; // NOLINT
          
      25  TEST(  collision_footprint,   test_basic )
          {
           nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0,   0,   0 );
          
           geometry_msgs::msg::Point p1;
           p1.x = -0.5;
           p1.y = 0.0;
           geometry_msgs::msg::Point p2;
           p2.x = 0.0;
           p2.y = 0.5;
           geometry_msgs::msg::Point p3;
           p3.x = 0.5;
           p3.y = 0.0;
           geometry_msgs::msg::Point p4;
           p4.x = 0.0;
           p4.y = -0.5;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_smac_planner::GridCollisionChecker collision_checker(  costmap_,   72 );
           collision_checker.setFootprint(  footprint,   false /*use footprint*/,   0.0 );
           collision_checker.inCollision(  5.0,   5.0,   0.0,   false );
           float cost = collision_checker.getCost(   );
           EXPECT_NEAR(  cost,   0.0,   0.001 );
           delete costmap_;
          }
          
      52  TEST(  collision_footprint,   test_point_cost )
          {
           nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0,   0,   0 );
          
           nav2_smac_planner::GridCollisionChecker collision_checker(  costmap_,   72 );
           nav2_costmap_2d::Footprint footprint;
           collision_checker.setFootprint(  footprint,   true /*radius / pointcose*/,   0.0 );
          
           collision_checker.inCollision(  5.0,   5.0,   0.0,   false );
           float cost = collision_checker.getCost(   );
           EXPECT_NEAR(  cost,   0.0,   0.001 );
           delete costmap_;
          }
          
      66  TEST(  collision_footprint,   test_world_to_map )
          {
           nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0,   0,   0 );
          
           nav2_smac_planner::GridCollisionChecker collision_checker(  costmap_,   72 );
           nav2_costmap_2d::Footprint footprint;
           collision_checker.setFootprint(  footprint,   true /*radius / point cost*/,   0.0 );
          
           unsigned int x,   y;
          
           collision_checker.worldToMap(  1.0,   1.0,   x,   y );
          
           collision_checker.inCollision(  x,   y,   0.0,   false );
           float cost = collision_checker.getCost(   );
          
           EXPECT_NEAR(  cost,   0.0,   0.001 );
          
           costmap_->setCost(  50,   50,   200 );
           collision_checker.worldToMap(  5.0,   5.0,   x,   y );
          
           collision_checker.inCollision(  x,   y,   0.0,   false );
           EXPECT_NEAR(  collision_checker.getCost(   ),   200.0,   0.001 );
           delete costmap_;
          }
          
      91  TEST(  collision_footprint,   test_footprint_at_pose_with_movement )
          {
           nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(  100,   100,   0.1,   0,   0,   254 );
          
           for (  unsigned int i = 40; i <= 60; ++i ) {
           for (  unsigned int j = 40; j <= 60; ++j ) {
           costmap_->setCost(  i,   j,   128 );
           }
           }
          
           geometry_msgs::msg::Point p1;
           p1.x = -1.0;
           p1.y = 1.0;
           geometry_msgs::msg::Point p2;
           p2.x = 1.0;
           p2.y = 1.0;
           geometry_msgs::msg::Point p3;
           p3.x = 1.0;
           p3.y = -1.0;
           geometry_msgs::msg::Point p4;
           p4.x = -1.0;
           p4.y = -1.0;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_smac_planner::GridCollisionChecker collision_checker(  costmap_,   72 );
           collision_checker.setFootprint(  footprint,   false /*use footprint*/,   0.0 );
          
           collision_checker.inCollision(  50,   50,   0.0,   false );
           float cost = collision_checker.getCost(   );
           EXPECT_NEAR(  cost,   128.0,   0.001 );
          
           collision_checker.inCollision(  50,   49,   0.0,   false );
           float up_value = collision_checker.getCost(   );
           EXPECT_NEAR(  up_value,   254.0,   0.001 );
          
           collision_checker.inCollision(  50,   52,   0.0,   false );
           float down_value = collision_checker.getCost(   );
           EXPECT_NEAR(  down_value,   254.0,   0.001 );
           delete costmap_;
          }
          
     133  TEST(  collision_footprint,   test_point_and_line_cost )
          {
           nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(  
           100,   100,   0.10000,   0,   0.0,   128.0 );
          
           costmap_->setCost(  62,   50,   254 );
           costmap_->setCost(  39,   60,   254 );
          
           geometry_msgs::msg::Point p1;
           p1.x = -1.0;
           p1.y = 1.0;
           geometry_msgs::msg::Point p2;
           p2.x = 1.0;
           p2.y = 1.0;
           geometry_msgs::msg::Point p3;
           p3.x = 1.0;
           p3.y = -1.0;
           geometry_msgs::msg::Point p4;
           p4.x = -1.0;
           p4.y = -1.0;
          
           nav2_costmap_2d::Footprint footprint = {p1,   p2,   p3,   p4};
          
           nav2_smac_planner::GridCollisionChecker collision_checker(  costmap_,   72 );
           collision_checker.setFootprint(  footprint,   false /*use footprint*/,   0.0 );
          
           collision_checker.inCollision(  50,   50,   0.0,   false );
           float value = collision_checker.getCost(   );
           EXPECT_NEAR(  value,   128.0,   0.001 );
          
           collision_checker.inCollision(  49,   50,   0.0,   false );
           float left_value = collision_checker.getCost(   );
           EXPECT_NEAR(  left_value,   254.0,   0.001 );
          
           collision_checker.inCollision(  52,   50,   0.0,   false );
           float right_value = collision_checker.getCost(   );
           EXPECT_NEAR(  right_value,   254.0,   0.001 );
           delete costmap_;
          }

navigation2/nav2_smac_planner/test/test_costmap_downsampler.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/costmap_downsampler.hpp"
          
      24  class RclCppFixture
          {
          public:
      27   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      28   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      32  TEST(  CostmapDownsampler,   costmap_downsample_test )
          {
           nav2_util::LifecycleNode::SharedPtr node = std::make_shared<nav2_util::LifecycleNode>(  
           "CostmapDownsamplerTest" );
           nav2_smac_planner::CostmapDownsampler downsampler;
          
           // create basic costmap
           nav2_costmap_2d::Costmap2D costmapA(  10,   10,   0.05,   0.0,   0.0,   0 );
           costmapA.setCost(  0,   0,   100 );
           costmapA.setCost(  5,   5,   50 );
          
           // downsample it
           downsampler.on_configure(  node,   "map",   "unused_topic",   &costmapA,   2 );
           nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(  2 );
          
           // validate it
           EXPECT_EQ(  downsampledCostmapA->getCost(  0,   0 ),   100 );
           EXPECT_EQ(  downsampledCostmapA->getCost(  2,   2 ),   50 );
           EXPECT_EQ(  downsampledCostmapA->getSizeInCellsX(   ),   5u );
           EXPECT_EQ(  downsampledCostmapA->getSizeInCellsY(   ),   5u );
          
           // give it another costmap of another size
           nav2_costmap_2d::Costmap2D costmapB(  4,   4,   0.10,   0.0,   0.0,   0 );
          
           // downsample it
           downsampler.on_configure(  node,   "map",   "unused_topic",   &costmapB,   4 );
           downsampler.on_activate(   );
           nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(  4 );
           downsampler.on_deactivate(   );
          
           // validate size
           EXPECT_EQ(  downsampledCostmapB->getSizeInCellsX(   ),   1u );
           EXPECT_EQ(  downsampledCostmapB->getSizeInCellsY(   ),   1u );
          
           downsampler.resizeCostmap(   );
          }

navigation2/nav2_smac_planner/test/test_node2d.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/node_2d.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          
      27  class RclCppFixture
          {
          public:
      30   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      31   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      35  TEST(  Node2DTest,   test_node_2d )
          {
           nav2_costmap_2d::Costmap2D costmapA(  10,   10,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  &costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // test construction
           unsigned char cost = static_cast<unsigned char>(  1 );
           nav2_smac_planner::Node2D testA(  1 );
           testA.setCost(  cost );
           nav2_smac_planner::Node2D testB(  1 );
           testB.setCost(  cost );
           EXPECT_EQ(  testA.getCost(   ),   1.0f );
           nav2_smac_planner::SearchInfo info;
           info.cost_penalty = 1.0;
           unsigned int size = 10;
           nav2_smac_planner::Node2D::initMotionModel(  
           nav2_smac_planner::MotionModel::MOORE,   size,   size,   size,   info );
          
           // test reset
           testA.reset(   );
           EXPECT_TRUE(  std::isnan(  testA.getCost(   ) ) );
          
           // check collision checking
           EXPECT_EQ(  testA.isNodeValid(  false,   checker.get(   ) ),   true );
           testA.setCost(  255 );
           EXPECT_EQ(  testA.isNodeValid(  true,   checker.get(   ) ),   true );
           testA.setCost(  10 );
          
           // check traversal cost computation
           EXPECT_NEAR(  testB.getTraversalCost(  &testA ),   1.03f,   0.1f );
          
           // check heuristic cost computation
           nav2_smac_planner::Node2D::Coordinates A(  0.0,   0.0 );
           nav2_smac_planner::Node2D::Coordinates B(  10.0,   5.0 );
           EXPECT_NEAR(  testB.getHeuristicCost(  A,   B,   nullptr ),   15.,   0.01 );
          
           // check operator== works on index
           unsigned char costC = '2';
           nav2_smac_planner::Node2D testC(  1 );
           testC.setCost(  costC );
           EXPECT_TRUE(  testA == testC );
          
           // check accumulated costs are set
           testC.setAccumulatedCost(  100 );
           EXPECT_EQ(  testC.getAccumulatedCost(   ),   100.0f );
          
           // check visiting state
           EXPECT_EQ(  testC.wasVisited(   ),   false );
           testC.queued(   );
           EXPECT_EQ(  testC.isQueued(   ),   true );
           testC.visited(   );
           EXPECT_EQ(  testC.wasVisited(   ),   true );
           EXPECT_EQ(  testC.isQueued(   ),   false );
          
           // check index
           EXPECT_EQ(  testC.getIndex(   ),   1u );
          
           // check static index functions
           EXPECT_EQ(  nav2_smac_planner::Node2D::getIndex(  1u,   1u,   10u ),   11u );
           EXPECT_EQ(  nav2_smac_planner::Node2D::getIndex(  6u,   43u,   10u ),   436u );
           EXPECT_EQ(  nav2_smac_planner::Node2D::getCoords(  436u,   10u,   1u ).x,   6u );
           EXPECT_EQ(  nav2_smac_planner::Node2D::getCoords(  436u,   10u,   1u ).y,   43u );
           EXPECT_THROW(  nav2_smac_planner::Node2D::getCoords(  436u,   10u,   10u ),   std::runtime_error );
          }
          
     102  TEST(  Node2DTest,   test_node_2d_neighbors )
          {
           nav2_smac_planner::SearchInfo info;
           unsigned int size_x = 10u;
           unsigned int size_y = 10u;
           unsigned int quant = 0u;
           // test neighborhood computation
           nav2_smac_planner::Node2D::initMotionModel(  
           nav2_smac_planner::MotionModel::VON_NEUMANN,   size_x,  
           size_y,   quant,   info );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(   ),   4u );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[0],   -1 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[1],   1 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[2],   -10 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[3],   10 );
          
           size_x = 100u;
           nav2_smac_planner::Node2D::initMotionModel(  
           nav2_smac_planner::MotionModel::MOORE,   size_x,   size_y,  
           quant,   info );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(   ),   8u );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[0],   -1 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[1],   1 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[2],   -100 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[3],   100 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[4],   -101 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[5],   -99 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[6],   99 );
           EXPECT_EQ(  nav2_smac_planner::Node2D::_neighbors_grid_offsets[7],   101 );
          
           nav2_costmap_2d::Costmap2D costmapA(  10,   10,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  &costmapA,   72 );
           unsigned char cost = static_cast<unsigned int>(  1 );
           nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(  1 );
           node->setCost(  cost );
           std::function<bool(  const unsigned int &,   nav2_smac_planner::Node2D * & )> neighborGetter =
           [&,   this](  const unsigned int & index,   nav2_smac_planner::Node2D * & neighbor_rtn ) -> bool
           {
           return false;
           };
          
           nav2_smac_planner::Node2D::NodeVector neighbors;
           node->getNeighbors(  neighborGetter,   checker.get(   ),   false,   neighbors );
           delete node;
          
           // should be empty since totally invalid
           EXPECT_EQ(  neighbors.size(   ),   0u );
          }

navigation2/nav2_smac_planner/test/test_nodebasic.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/node_basic.hpp"
          #include "nav2_smac_planner/node_2d.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          
      31  class RclCppFixture
          {
          public:
      34   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      35   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      39  TEST(  NodeBasicTest,   test_node_basic )
          {
           nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeHybrid> node(  50 );
          
           EXPECT_EQ(  node.index,   50u );
           EXPECT_EQ(  node.graph_node_ptr,   nullptr );
          
           nav2_smac_planner::NodeBasic<nav2_smac_planner::Node2D> node2(  100 );
          
           EXPECT_EQ(  node2.index,   100u );
           EXPECT_EQ(  node2.graph_node_ptr,   nullptr );
          
           nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeLattice> node3(  200 );
          
           EXPECT_EQ(  node3.index,   200u );
           EXPECT_EQ(  node3.graph_node_ptr,   nullptr );
          }

navigation2/nav2_smac_planner/test/test_nodehybrid.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          
      28  class RclCppFixture
          {
          public:
      31   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      32   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      36  TEST(  NodeHybridTest,   test_node_hybrid )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.1;
           info.non_straight_penalty = 1.1;
           info.reverse_penalty = 2.0;
           info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
           info.cost_penalty = 1.7;
           info.retrospective_penalty = 0.1;
           unsigned int size_x = 10;
           unsigned int size_y = 10;
           unsigned int size_theta = 72;
          
           // Check defaulted constants
           nav2_smac_planner::NodeHybrid testA(  49 );
           EXPECT_EQ(  testA.travel_distance_cost,   sqrt(  2 ) );
          
           nav2_smac_planner::NodeHybrid::initMotionModel(  
           nav2_smac_planner::MotionModel::DUBIN,   size_x,   size_y,   size_theta,   info );
          
           nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(  
           10,   10,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // test construction
           nav2_smac_planner::NodeHybrid testB(  49 );
           EXPECT_TRUE(  std::isnan(  testA.getCost(   ) ) );
          
           // test node valid and cost
           testA.pose.x = 5;
           testA.pose.y = 5;
           testA.pose.theta = 0;
           EXPECT_EQ(  testA.isNodeValid(  true,   checker.get(   ) ),   true );
           EXPECT_EQ(  testA.isNodeValid(  false,   checker.get(   ) ),   true );
           EXPECT_EQ(  testA.getCost(   ),   0.0f );
          
           // test reset
           testA.reset(   );
           EXPECT_TRUE(  std::isnan(  testA.getCost(   ) ) );
          
           // Check motion-specific constants
           EXPECT_NEAR(  testA.travel_distance_cost,   2.08842,   0.1 );
          
           // check collision checking
           EXPECT_EQ(  testA.isNodeValid(  false,   checker.get(   ) ),   true );
          
           // check traversal cost computation
           // simulated first node,   should return neutral cost
           EXPECT_NEAR(  testB.getTraversalCost(  &testA ),   2.088,   0.1 );
           // now with straight motion,   cost is 0,   so will be neutral as well
           // but now reduced by retrospective penalty (  10% )
           testB.setMotionPrimitiveIndex(  1 );
           testA.setMotionPrimitiveIndex(  0 );
           EXPECT_NEAR(  testB.getTraversalCost(  &testA ),   2.088 * 0.9,   0.1 );
           // same direction as parent,   testB
           testA.setMotionPrimitiveIndex(  1 );
           EXPECT_NEAR(  testB.getTraversalCost(  &testA ),   2.297f * 0.9,   0.01 );
           // opposite direction as parent,   testB
           testA.setMotionPrimitiveIndex(  2 );
           EXPECT_NEAR(  testB.getTraversalCost(  &testA ),   2.506f * 0.9,   0.01 );
          
           // will throw because never collision checked testB
           EXPECT_THROW(  testA.getTraversalCost(  &testB ),   std::runtime_error );
          
           // check motion primitives
           EXPECT_EQ(  testA.getMotionPrimitiveIndex(   ),   2u );
          
           // check operator== works on index
           nav2_smac_planner::NodeHybrid testC(  49 );
           EXPECT_TRUE(  testA == testC );
          
           // check accumulated costs are set
           testC.setAccumulatedCost(  100 );
           EXPECT_EQ(  testC.getAccumulatedCost(   ),   100.0f );
          
           // check visiting state
           EXPECT_EQ(  testC.wasVisited(   ),   false );
           testC.visited(   );
           EXPECT_EQ(  testC.wasVisited(   ),   true );
          
           // check index
           EXPECT_EQ(  testC.getIndex(   ),   49u );
          
           // check set pose and pose
           testC.setPose(  nav2_smac_planner::NodeHybrid::Coordinates(  10.0,   5.0,   4 ) );
           EXPECT_EQ(  testC.pose.x,   10.0 );
           EXPECT_EQ(  testC.pose.y,   5.0 );
           EXPECT_EQ(  testC.pose.theta,   4 );
          
           // check static index functions
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::getIndex(  1u,   1u,   4u,   10u,   72u ),   796u );
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::getCoords(  796u,   10u,   72u ).x,   1u );
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::getCoords(  796u,   10u,   72u ).y,   1u );
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::getCoords(  796u,   10u,   72u ).theta,   4u );
          
           delete costmapA;
          }
          
     136  TEST(  NodeHybridTest,   test_obstacle_heuristic )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.1;
           info.non_straight_penalty = 1.1;
           info.reverse_penalty = 2.0;
           info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
           info.cost_penalty = 1.7;
           info.retrospective_penalty = 0.0;
           unsigned int size_x = 100;
           unsigned int size_y = 100;
           unsigned int size_theta = 72;
          
           nav2_smac_planner::NodeHybrid::initMotionModel(  
           nav2_smac_planner::MotionModel::DUBIN,   size_x,   size_y,   size_theta,   info );
          
           nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(  
           100,   100,   0.1,   0.0,   0.0,   0 );
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 20; i <= 80; ++i ) {
           for (  unsigned int j = 40; j <= 60; ++j ) {
           costmapA->setCost(  i,   j,   254 );
           }
           }
           // path on the right is narrow and thus with high cost
           for (  unsigned int i = 20; i <= 80; ++i ) {
           for (  unsigned int j = 61; j <= 70; ++j ) {
           costmapA->setCost(  i,   j,   250 );
           }
           }
           for (  unsigned int i = 20; i <= 80; ++i ) {
           for (  unsigned int j = 71; j < 100; ++j ) {
           costmapA->setCost(  i,   j,   254 );
           }
           }
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           nav2_smac_planner::NodeHybrid testA(  0 );
           testA.pose.x = 10;
           testA.pose.y = 50;
           testA.pose.theta = 0;
          
           nav2_smac_planner::NodeHybrid testB(  1 );
           testB.pose.x = 90;
           testB.pose.y = 51; // goal is a bit closer to the high-cost passage
           testB.pose.theta = 0;
          
           // first block the high-cost passage to make sure the cost spreads through the better path
           for (  unsigned int j = 61; j <= 70; ++j ) {
           costmapA->setCost(  50,   j,   254 );
           }
           nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(  
           costmapA,   testA.pose.x,   testA.pose.y,   testB.pose.x,   testB.pose.y );
           float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(  
           testA.pose,  
           testB.pose,  
           info.cost_penalty );
          
           EXPECT_NEAR(  wide_passage_cost,   91.1f,   0.1f );
          
           // then unblock it to check if cost remains the same
           // (  it should,   since the unblocked narrow path will have higher cost than the wide one
           // and thus lower bound of the path cost should be unchanged )
           for (  unsigned int j = 61; j <= 70; ++j ) {
           costmapA->setCost(  50,   j,   250 );
           }
           nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(  
           costmapA,  
           testA.pose.x,   testA.pose.y,   testB.pose.x,   testB.pose.y );
           float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(  
           testA.pose,  
           testB.pose,  
           info.cost_penalty );
          
           EXPECT_EQ(  wide_passage_cost,   two_passages_cost );
          
           delete costmapA;
          }
          
     217  TEST(  NodeHybridTest,   test_node_debin_neighbors )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 1.2;
           info.non_straight_penalty = 1.4;
           info.reverse_penalty = 2.1;
           info.minimum_turning_radius = 4; // 0.2 in grid coordinates
           info.retrospective_penalty = 0.0;
           unsigned int size_x = 100;
           unsigned int size_y = 100;
           unsigned int size_theta = 72;
           nav2_smac_planner::NodeHybrid::initMotionModel(  
           nav2_smac_planner::MotionModel::DUBIN,   size_x,   size_y,   size_theta,   info );
          
           // test neighborhood computation
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::motion_table.projections.size(   ),   3u );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x,   1.731517,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y,   0,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta,   0,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x,   1.69047,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y,   0.3747,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta,   5,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x,   1.69047,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y,   -0.3747,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta,   -5,   0.01 );
          }
          
     246  TEST(  NodeHybridTest,   test_node_reeds_neighbors )
          {
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 1.2;
           info.non_straight_penalty = 1.4;
           info.reverse_penalty = 2.1;
           info.minimum_turning_radius = 8; // 0.4 in grid coordinates
           info.retrospective_penalty = 0.0;
           unsigned int size_x = 100;
           unsigned int size_y = 100;
           unsigned int size_theta = 72;
           nav2_smac_planner::NodeHybrid::initMotionModel(  
           nav2_smac_planner::MotionModel::REEDS_SHEPP,   size_x,   size_y,   size_theta,   info );
          
           EXPECT_EQ(  nav2_smac_planner::NodeHybrid::motion_table.projections.size(   ),   6u );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x,   2.088,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y,   0,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta,   0,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x,   2.070,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y,   0.272,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta,   3,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x,   2.070,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y,   -0.272,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta,   -3,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x,   -2.088,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y,   0,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta,   0,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x,   -2.07,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y,   0.272,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta,   -3,   0.01 );
          
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x,   -2.07,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y,   -0.272,   0.01 );
           EXPECT_NEAR(  nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta,   3,   0.01 );
          
           nav2_costmap_2d::Costmap2D costmapA(  100,   100,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  &costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
           nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(  49 );
           std::function<bool(  const unsigned int &,   nav2_smac_planner::NodeHybrid * & )> neighborGetter =
           [&,   this](  const unsigned int & index,   nav2_smac_planner::NodeHybrid * & neighbor_rtn ) -> bool
           {
           // because we don't return a real object
           return false;
           };
          
           nav2_smac_planner::NodeHybrid::NodeVector neighbors;
           node->getNeighbors(  neighborGetter,   checker.get(   ),   false,   neighbors );
           delete node;
          
           // should be empty since totally invalid
           EXPECT_EQ(  neighbors.size(   ),   0u );
          }

navigation2/nav2_smac_planner/test/test_nodelattice.cpp

       1  // Copyright (  c ) 2021 Joshua Wallace
          // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <fstream>
          #include <string>
          #include <vector>
          #include <memory>
          #include <unordered_map>
          #include <limits>
          #include "nav2_smac_planner/node_lattice.hpp"
          #include "gtest/gtest.h"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          using json = nlohmann::json;
          
      28  TEST(  NodeLatticeTest,   parser_test )
          {
           std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" );
           std::string filePath =
           pkg_share_dir +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
           std::ifstream myJsonFile(  filePath );
          
           ASSERT_TRUE(  myJsonFile.is_open(   ) );
          
           json j;
           myJsonFile >> j;
          
           nav2_smac_planner::LatticeMetadata metaData;
           nav2_smac_planner::MotionPrimitive myPrimitive;
           nav2_smac_planner::MotionPose pose;
          
           json jsonMetaData = j["lattice_metadata"];
           json jsonPrimatives = j["primitives"];
           json jsonPose = jsonPrimatives[0]["poses"][0];
          
           nav2_smac_planner::fromJsonToMetaData(  jsonMetaData,   metaData );
          
           // Checks for parsing meta data
           EXPECT_NEAR(  metaData.min_turning_radius,   0.5,   0.001 );
           EXPECT_NEAR(  metaData.grid_resolution,   0.05,   0.001 );
           EXPECT_NEAR(  metaData.number_of_headings,   16,   0.01 );
           EXPECT_NEAR(  metaData.heading_angles[0],   0.0,   0.01 );
           EXPECT_EQ(  metaData.number_of_trajectories,   80u );
           EXPECT_EQ(  metaData.motion_model,   std::string(  "ackermann" ) );
          
           std::vector<nav2_smac_planner::MotionPrimitive> myPrimitives;
           for (  unsigned int i = 0; i < jsonPrimatives.size(   ); ++i ) {
           nav2_smac_planner::MotionPrimitive newPrimative;
           nav2_smac_planner::fromJsonToMotionPrimitive(  jsonPrimatives[i],   newPrimative );
           myPrimitives.push_back(  newPrimative );
           }
          
           // Checks for parsing primitives
           EXPECT_EQ(  myPrimitives.size(   ),   80u );
           EXPECT_NEAR(  myPrimitives[0].trajectory_id,   0,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].start_angle,   0.0,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].end_angle,   13,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].turning_radius,   0.5259,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].trajectory_length,   0.64856,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].arc_length,   0.58225,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].straight_length,   0.06631,   0.01 );
          
           EXPECT_NEAR(  myPrimitives[0].poses[0]._x,   0.04981,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].poses[0]._y,   -0.00236,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].poses[0]._theta,   6.1883,   0.01 );
          
           EXPECT_NEAR(  myPrimitives[0].poses[1]._x,   0.09917,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].poses[1]._y,   -0.00944,   0.01 );
           EXPECT_NEAR(  myPrimitives[0].poses[1]._theta,   6.09345,   0.015 );
          }
          
      86  TEST(  NodeLatticeTest,   test_node_lattice_neighbors_and_parsing )
          {
           std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" );
           std::string filePath =
           pkg_share_dir +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
          
           nav2_smac_planner::SearchInfo info;
           info.minimum_turning_radius = 1.1;
           info.non_straight_penalty = 1;
           info.change_penalty = 1;
           info.reverse_penalty = 1;
           info.cost_penalty = 1;
           info.retrospective_penalty = 0.0;
           info.analytic_expansion_ratio = 1;
           info.lattice_filepath = filePath;
           info.cache_obstacle_heuristic = true;
           info.allow_reverse_expansion = true;
          
           unsigned int x = 100;
           unsigned int y = 100;
           unsigned int angle_quantization = 16;
          
           nav2_smac_planner::NodeLattice::initMotionModel(  
           nav2_smac_planner::MotionModel::STATE_LATTICE,   x,   y,   angle_quantization,   info );
          
           nav2_smac_planner::NodeLattice aNode(  0 );
           aNode.setPose(  nav2_smac_planner::NodeHybrid::Coordinates(  0,   0,   0 ) );
           nav2_smac_planner::MotionPrimitivePtrs projections =
           nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(  &aNode );
          
           EXPECT_NEAR(  projections[0]->poses.back(   )._x,   0.5,   0.01 );
           EXPECT_NEAR(  projections[0]->poses.back(   )._y,   -0.35,   0.01 );
           EXPECT_NEAR(  projections[0]->poses.back(   )._theta,   5.176,   0.01 );
          
           EXPECT_NEAR(  
           nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata(  
           filePath ).grid_resolution,   0.05,   0.005 );
          }
          
     127  TEST(  NodeLatticeTest,   test_node_lattice_conversions )
          {
           std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" );
           std::string filePath =
           pkg_share_dir +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
          
           nav2_smac_planner::SearchInfo info;
           info.minimum_turning_radius = 1.1;
           info.non_straight_penalty = 1;
           info.change_penalty = 1;
           info.reverse_penalty = 1;
           info.cost_penalty = 1;
           info.retrospective_penalty = 0.0;
           info.analytic_expansion_ratio = 1;
           info.lattice_filepath = filePath;
           info.cache_obstacle_heuristic = true;
          
           unsigned int x = 100;
           unsigned int y = 100;
           unsigned int angle_quantization = 16;
          
           nav2_smac_planner::NodeLattice::initMotionModel(  
           nav2_smac_planner::MotionModel::STATE_LATTICE,   x,   y,   angle_quantization,   info );
          
           nav2_smac_planner::NodeLattice aNode(  0 );
           aNode.setPose(  nav2_smac_planner::NodeHybrid::Coordinates(  0,   0,   0 ) );
          
           EXPECT_NEAR(  aNode.motion_table.getAngleFromBin(  0u ),   0.0,   0.005 );
           EXPECT_NEAR(  aNode.motion_table.getAngleFromBin(  1u ),   0.46364,   0.005 );
           EXPECT_NEAR(  aNode.motion_table.getAngleFromBin(  2u ),   0.78539,   0.005 );
          
           EXPECT_EQ(  aNode.motion_table.getClosestAngularBin(  0.0 ),   0u );
           EXPECT_EQ(  aNode.motion_table.getClosestAngularBin(  0.5 ),   1u );
           EXPECT_EQ(  aNode.motion_table.getClosestAngularBin(  1.5 ),   4u );
          }
          
     165  TEST(  NodeLatticeTest,   test_node_lattice )
          {
           std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" );
           std::string filePath =
           pkg_share_dir +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
          
           nav2_smac_planner::SearchInfo info;
           info.minimum_turning_radius = 1.1;
           info.non_straight_penalty = 1;
           info.change_penalty = 1;
           info.reverse_penalty = 1;
           info.cost_penalty = 1;
           info.retrospective_penalty = 0.1;
           info.analytic_expansion_ratio = 1;
           info.lattice_filepath = filePath;
           info.cache_obstacle_heuristic = true;
           info.allow_reverse_expansion = true;
          
           unsigned int x = 100;
           unsigned int y = 100;
           unsigned int angle_quantization = 16;
          
           nav2_smac_planner::NodeLattice::initMotionModel(  
           nav2_smac_planner::MotionModel::STATE_LATTICE,   x,   y,   angle_quantization,   info );
          
           // Check defaults
           nav2_smac_planner::NodeLattice aNode(  0 );
           nav2_smac_planner::NodeLattice testA(  49 );
           EXPECT_EQ(  testA.getIndex(   ),   49u );
           EXPECT_EQ(  testA.getAccumulatedCost(   ),   std::numeric_limits<float>::max(   ) );
           EXPECT_TRUE(  std::isnan(  testA.getCost(   ) ) );
           EXPECT_EQ(  testA.getMotionPrimitive(   ),   nullptr );
          
           // Test visited state / reset
           EXPECT_EQ(  testA.wasVisited(   ),   false );
           testA.visited(   );
           EXPECT_EQ(  testA.wasVisited(   ),   true );
           testA.reset(   );
           EXPECT_EQ(  testA.wasVisited(   ),   false );
          
           nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(  
           10,   10,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // test node valid and cost
           testA.pose.x = 5;
           testA.pose.y = 5;
           testA.pose.theta = 0;
           EXPECT_EQ(  testA.isNodeValid(  true,   checker.get(   ) ),   true );
           EXPECT_EQ(  testA.isNodeValid(  false,   checker.get(   ) ),   true );
           EXPECT_EQ(  testA.getCost(   ),   0.0f );
          
           // check collision checking
           EXPECT_EQ(  testA.isNodeValid(  false,   checker.get(   ) ),   true );
          
           // check operator== works on index
           nav2_smac_planner::NodeLattice testC(  49 );
           EXPECT_TRUE(  testA == testC );
          
           // check accumulated costs are set
           testC.setAccumulatedCost(  100 );
           EXPECT_EQ(  testC.getAccumulatedCost(   ),   100.0f );
          
           // check set pose and pose
           testC.setPose(  nav2_smac_planner::NodeLattice::Coordinates(  10.0,   5.0,   4 ) );
           EXPECT_EQ(  testC.pose.x,   10.0 );
           EXPECT_EQ(  testC.pose.y,   5.0 );
           EXPECT_EQ(  testC.pose.theta,   4 );
          
           delete costmapA;
          }
          
          
     242  TEST(  NodeLatticeTest,   test_get_neighbors )
          {
           std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(  "nav2_smac_planner" );
           std::string filePath =
           pkg_share_dir +
           "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
           "/output.json";
          
           nav2_smac_planner::SearchInfo info;
           info.minimum_turning_radius = 1.1;
           info.non_straight_penalty = 1;
           info.change_penalty = 1;
           info.reverse_penalty = 1;
           info.cost_penalty = 1;
           info.analytic_expansion_ratio = 1;
           info.retrospective_penalty = 0.0;
           info.lattice_filepath = filePath;
           info.cache_obstacle_heuristic = true;
           info.allow_reverse_expansion = true;
          
           unsigned int x = 100;
           unsigned int y = 100;
           unsigned int angle_quantization = 16;
          
           nav2_smac_planner::NodeLattice::initMotionModel(  
           nav2_smac_planner::MotionModel::STATE_LATTICE,   x,   y,   angle_quantization,   info );
          
           nav2_smac_planner::NodeLattice node(  49 );
          
           nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(  
           10,   10,   0.05,   0.0,   0.0,   0 );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmapA,   72 );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           std::function<bool(  const unsigned int &,   nav2_smac_planner::NodeLattice * & )> neighborGetter =
           [&,   this](  const unsigned int & index,   nav2_smac_planner::NodeLattice * & neighbor_rtn ) -> bool
           {
           // because we don't return a real object
           return false;
           };
          
           nav2_smac_planner::NodeLattice::NodeVector neighbors;
           node.getNeighbors(  neighborGetter,   checker.get(   ),   false,   neighbors );
           // should be empty since totally invalid
           EXPECT_EQ(  neighbors.size(   ),   0u );
          
           delete costmapA;
          }

navigation2/nav2_smac_planner/test/test_smac_2d.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "gtest/gtest.h"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/smac_planner_2d.hpp"
          #include "nav2_smac_planner/smac_planner_hybrid.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      33  class RclCppFixture
          {
          public:
      36   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      37   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
          // SMAC smoke tests for plugin-level issues rather than algorithms
          // (  covered by more extensively testing in other files )
          // System tests in nav2_system_tests will actually plan with this work
          
      45  TEST(  SmacTest,   test_smac_2d ) {
           rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "Smac2DTest" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           node2D->declare_parameter(  "test.smooth_path",   true );
           node2D->set_parameter(  rclcpp::Parameter(  "test.smooth_path",   true ) );
           node2D->declare_parameter(  "test.downsample_costmap",   true );
           node2D->set_parameter(  rclcpp::Parameter(  "test.downsample_costmap",   true ) );
           node2D->declare_parameter(  "test.downsampling_factor",   2 );
           node2D->set_parameter(  rclcpp::Parameter(  "test.downsampling_factor",   2 ) );
          
           geometry_msgs::msg::PoseStamped start,   goal;
           start.pose.position.x = 0.0;
           start.pose.position.y = 0.0;
           start.pose.orientation.w = 1.0;
           // goal = start;
           goal.pose.position.x = 7.0;
           goal.pose.position.y = 0.0;
           goal.pose.orientation.w = 1.0;
           auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>(   );
           planner_2d->configure(  node2D,   "test",   nullptr,   costmap_ros );
           planner_2d->activate(   );
           try {
           planner_2d->createPlan(  start,   goal );
           } catch (  ... ) {
           }
          
           planner_2d->deactivate(   );
           planner_2d->cleanup(   );
          
           planner_2d.reset(   );
           costmap_ros->on_cleanup(  rclcpp_lifecycle::State(   ) );
           node2D.reset(   );
           costmap_ros.reset(   );
          }
          
      85  TEST(  SmacTest,   test_smac_2d_reconfigure ) {
           rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "Smac2DTest" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>(   );
           planner_2d->configure(  node2D,   "test",   nullptr,   costmap_ros );
           planner_2d->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           node2D->get_node_base_interface(   ),   node2D->get_node_topics_interface(   ),  
           node2D->get_node_graph_interface(   ),  
           node2D->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.tolerance",   1.0 ),  
           rclcpp::Parameter(  "test.cost_travel_multiplier",   1.0 ),  
           rclcpp::Parameter(  "test.max_planning_time",   2.0 ),  
           rclcpp::Parameter(  "test.downsample_costmap",   false ),  
           rclcpp::Parameter(  "test.allow_unknown",   false ),  
           rclcpp::Parameter(  "test.downsampling_factor",   2 ),  
           rclcpp::Parameter(  "test.max_iterations",   -1 ),  
           rclcpp::Parameter(  "test.max_on_approach_iterations",   -1 ),  
           rclcpp::Parameter(  "test.motion_model_for_search",   "UNKNOWN" ),  
           rclcpp::Parameter(  "test.use_final_approach_orientation",   false )} );
          
           rclcpp::spin_until_future_complete(  
           node2D->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  node2D->get_parameter(  "test.tolerance" ).as_double(   ),   1.0 );
           EXPECT_EQ(  
           node2D->get_parameter(  "test.cost_travel_multiplier" ).as_double(   ),  
           1.0 );
           EXPECT_EQ(  node2D->get_parameter(  "test.max_planning_time" ).as_double(   ),   2.0 );
           EXPECT_EQ(  node2D->get_parameter(  "test.downsample_costmap" ).as_bool(   ),   false );
           EXPECT_EQ(  node2D->get_parameter(  "test.allow_unknown" ).as_bool(   ),   false );
           EXPECT_EQ(  node2D->get_parameter(  "test.downsampling_factor" ).as_int(   ),   2 );
           EXPECT_EQ(  node2D->get_parameter(  "test.max_iterations" ).as_int(   ),   -1 );
           EXPECT_EQ(  node2D->get_parameter(  "test.use_final_approach_orientation" ).as_bool(   ),   false );
           EXPECT_EQ(  
           node2D->get_parameter(  "test.max_on_approach_iterations" ).as_int(   ),  
           -1 );
           EXPECT_EQ(  
           node2D->get_parameter(  "test.motion_model_for_search" ).as_string(   ),  
           "UNKNOWN" );
          
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.downsample_costmap",   true )} );
          
           rclcpp::spin_until_future_complete(  
           node2D->get_node_base_interface(   ),  
           results );
          }

navigation2/nav2_smac_planner/test/test_smac_hybrid.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/smac_planner_hybrid.hpp"
          #include "nav2_smac_planner/smac_planner_2d.hpp"
          
      32  class RclCppFixture
          {
          public:
      35   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      36   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
          // SMAC smoke tests for plugin-level issues rather than algorithms
          // (  covered by more extensively testing in other files )
          // System tests in nav2_system_tests will actually plan with this work
          
      44  TEST(  SmacTest,   test_smac_se2 )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacSE2Test" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           nodeSE2->declare_parameter(  "test.downsample_costmap",   true );
           nodeSE2->set_parameter(  rclcpp::Parameter(  "test.downsample_costmap",   true ) );
           nodeSE2->declare_parameter(  "test.downsampling_factor",   2 );
           nodeSE2->set_parameter(  rclcpp::Parameter(  "test.downsampling_factor",   2 ) );
          
           geometry_msgs::msg::PoseStamped start,   goal;
           start.pose.position.x = 0.0;
           start.pose.position.y = 0.0;
           start.pose.orientation.w = 1.0;
           goal.pose.position.x = 1.0;
           goal.pose.position.y = 1.0;
           goal.pose.orientation.w = 1.0;
           auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>(   );
           planner->configure(  nodeSE2,   "test",   nullptr,   costmap_ros );
           planner->activate(   );
          
           try {
           planner->createPlan(  start,   goal );
           } catch (  ... ) {
           }
          
           planner->deactivate(   );
           planner->cleanup(   );
          
           planner.reset(   );
           costmap_ros->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap_ros.reset(   );
           nodeSE2.reset(   );
          }
          
      83  TEST(  SmacTest,   test_smac_se2_reconfigure )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacSE2Test" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>(   );
           planner->configure(  nodeSE2,   "test",   nullptr,   costmap_ros );
           planner->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           nodeSE2->get_node_base_interface(   ),   nodeSE2->get_node_topics_interface(   ),  
           nodeSE2->get_node_graph_interface(   ),  
           nodeSE2->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.downsample_costmap",   true ),  
           rclcpp::Parameter(  "test.downsampling_factor",   2 ),  
           rclcpp::Parameter(  "test.angle_quantization_bins",   100 ),  
           rclcpp::Parameter(  "test.allow_unknown",   false ),  
           rclcpp::Parameter(  "test.max_iterations",   -1 ),  
           rclcpp::Parameter(  "test.minimum_turning_radius",   1.0 ),  
           rclcpp::Parameter(  "test.cache_obstacle_heuristic",   true ),  
           rclcpp::Parameter(  "test.reverse_penalty",   5.0 ),  
           rclcpp::Parameter(  "test.change_penalty",   1.0 ),  
           rclcpp::Parameter(  "test.non_straight_penalty",   2.0 ),  
           rclcpp::Parameter(  "test.cost_penalty",   2.0 ),  
           rclcpp::Parameter(  "test.retrospective_penalty",   0.2 ),  
           rclcpp::Parameter(  "test.analytic_expansion_ratio",   4.0 ),  
           rclcpp::Parameter(  "test.max_planning_time",   10.0 ),  
           rclcpp::Parameter(  "test.lookup_table_size",   30.0 ),  
           rclcpp::Parameter(  "test.smooth_path",   false ),  
           rclcpp::Parameter(  "test.analytic_expansion_max_length",   42.0 ),  
           rclcpp::Parameter(  "test.motion_model_for_search",   std::string(  "REEDS_SHEPP" ) )} );
          
           rclcpp::spin_until_future_complete(  
           nodeSE2->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.downsample_costmap" ).as_bool(   ),   true );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.downsampling_factor" ).as_int(   ),   2 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.angle_quantization_bins" ).as_int(   ),   100 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.allow_unknown" ).as_bool(   ),   false );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.max_iterations" ).as_int(   ),   -1 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.minimum_turning_radius" ).as_double(   ),   1.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.cache_obstacle_heuristic" ).as_bool(   ),   true );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.reverse_penalty" ).as_double(   ),   5.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.change_penalty" ).as_double(   ),   1.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.non_straight_penalty" ).as_double(   ),   2.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.cost_penalty" ).as_double(   ),   2.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.retrospective_penalty" ).as_double(   ),   0.2 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.analytic_expansion_ratio" ).as_double(   ),   4.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.smooth_path" ).as_bool(   ),   false );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.max_planning_time" ).as_double(   ),   10.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.lookup_table_size" ).as_double(   ),   30.0 );
           EXPECT_EQ(  nodeSE2->get_parameter(  "test.analytic_expansion_max_length" ).as_double(   ),   42.0 );
           EXPECT_EQ(  
           nodeSE2->get_parameter(  "test.motion_model_for_search" ).as_string(   ),  
           std::string(  "REEDS_SHEPP" ) );
          }

navigation2/nav2_smac_planner/test/test_smac_lattice.cpp

       1  // Copyright (  c ) 2021 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/smac_planner_lattice.hpp"
          
      31  class RclCppFixture
          {
          public:
      34   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      35   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
          // Simple wrapper to be able to call a private member
      40  class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice
          {
          public:
      43   void callDynamicParams(  std::vector<rclcpp::Parameter> parameters )
           {
           dynamicParametersCallback(  parameters );
           }
          };
          
          // SMAC smoke tests for plugin-level issues rather than algorithms
          // (  covered by more extensively testing in other files )
          // System tests in nav2_system_tests will actually plan with this work
          
      53  TEST(  SmacTest,   test_smac_lattice )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacLatticeTest" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           geometry_msgs::msg::PoseStamped start,   goal;
           start.pose.position.x = 0.0;
           start.pose.position.y = 0.0;
           start.pose.orientation.w = 1.0;
           goal.pose.position.x = 1.0;
           goal.pose.position.y = 1.0;
           goal.pose.orientation.w = 1.0;
           auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>(   );
           try {
           // Expect to throw due to invalid prims file in param
           planner->configure(  nodeLattice,   "test",   nullptr,   costmap_ros );
           } catch (  ... ) {
           }
           planner->activate(   );
          
           try {
           planner->createPlan(  start,   goal );
           } catch (  ... ) {
           }
          
           planner->deactivate(   );
           planner->cleanup(   );
          
           planner.reset(   );
           costmap_ros->on_cleanup(  rclcpp_lifecycle::State(   ) );
           costmap_ros.reset(   );
           nodeLattice.reset(   );
          }
          
      91  TEST(  SmacTest,   test_smac_lattice_reconfigure )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacLatticeTest" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           auto planner = std::make_unique<LatticeWrap>(   );
           try {
           // Expect to throw due to invalid prims file in param
           planner->configure(  nodeLattice,   "test",   nullptr,   costmap_ros );
           } catch (  ... ) {
           }
           planner->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           nodeLattice->get_node_base_interface(   ),   nodeLattice->get_node_topics_interface(   ),  
           nodeLattice->get_node_graph_interface(   ),  
           nodeLattice->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "test.allow_unknown",   false ),  
           rclcpp::Parameter(  "test.max_iterations",   -1 ),  
           rclcpp::Parameter(  "test.cache_obstacle_heuristic",   true ),  
           rclcpp::Parameter(  "test.reverse_penalty",   5.0 ),  
           rclcpp::Parameter(  "test.change_penalty",   1.0 ),  
           rclcpp::Parameter(  "test.non_straight_penalty",   2.0 ),  
           rclcpp::Parameter(  "test.cost_penalty",   2.0 ),  
           rclcpp::Parameter(  "test.retrospective_penalty",   0.2 ),  
           rclcpp::Parameter(  "test.analytic_expansion_ratio",   4.0 ),  
           rclcpp::Parameter(  "test.max_planning_time",   10.0 ),  
           rclcpp::Parameter(  "test.lookup_table_size",   30.0 ),  
           rclcpp::Parameter(  "test.smooth_path",   false ),  
           rclcpp::Parameter(  "test.analytic_expansion_max_length",   42.0 ),  
           rclcpp::Parameter(  "test.rotation_penalty",   42.0 ),  
           rclcpp::Parameter(  "test.allow_reverse_expansion",   true )} );
          
           try {
           // All of these params will re-init A* which will involve loading the control set file
           // which will cause an exception because the file does not exist. This will cause an
           // expected failure preventing parameter updates from being successfully processed
           rclcpp::spin_until_future_complete(  
           nodeLattice->get_node_base_interface(   ),  
           results );
           } catch (  ... ) {
           }
          
           // So instead,   lets call manually on a change
           std::vector<rclcpp::Parameter> parameters;
           parameters.push_back(  rclcpp::Parameter(  "test.lattice_filepath",   std::string(  "HI" ) ) );
           EXPECT_THROW(  planner->callDynamicParams(  parameters ),   std::runtime_error );
          }

navigation2/nav2_smac_planner/test/test_smoother.cpp

       1  // Copyright (  c ) 2020,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smac_planner/node_hybrid.hpp"
          #include "nav2_smac_planner/a_star.hpp"
          #include "nav2_smac_planner/collision_checker.hpp"
          #include "nav2_smac_planner/smoother.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          using namespace nav2_smac_planner; // NOLINT
          
      34  class RclCppFixture
          {
          public:
      37   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      38   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      42  class SmootherWrapper : public nav2_smac_planner::Smoother
          {
          public:
      45   explicit SmootherWrapper(  const SmootherParams & params )
           : nav2_smac_planner::Smoother(  params )
           {}
          
      49   std::vector<PathSegment> findDirectionalPathSegmentsWrapper(  nav_msgs::msg::Path path )
           {
           return findDirectionalPathSegments(  path );
           }
          };
          
      55  TEST(  SmootherTest,   test_full_smoother )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr node =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacSmootherTest" );
           nav2_smac_planner::SmootherParams params;
           params.get(  node,   "test" );
           double maxtime = 1.0;
          
           // Make smoother and costmap to smooth in
           auto smoother = std::make_unique<SmootherWrapper>(  params );
           smoother->initialize(  0.4 /*turning radius*/ );
          
           nav2_costmap_2d::Costmap2D * costmap =
           new nav2_costmap_2d::Costmap2D(  100,   100,   0.05,   0.0,   0.0,   0 );
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 20; i <= 30; ++i ) {
           for (  unsigned int j = 20; j <= 30; ++j ) {
           costmap->setCost(  i,   j,   254 );
           }
           }
          
           // Setup A* search to get path to smooth
           nav2_smac_planner::SearchInfo info;
           info.change_penalty = 0.05;
           info.non_straight_penalty = 1.05;
           info.reverse_penalty = 2.0;
           info.cost_penalty = 2.0;
           info.retrospective_penalty = 0.0;
           info.analytic_expansion_ratio = 3.5;
           info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
           info.analytic_expansion_max_length = 20.0; // in grid coordinates
           unsigned int size_theta = 72;
           nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(  
           nav2_smac_planner::MotionModel::REEDS_SHEPP,   info );
           int max_iterations = 10000;
           float tolerance = 10.0;
           int it_on_approach = 10;
           double max_planning_time = 120.0;
           int num_it = 0;
          
           a_star.initialize(  
           false,   max_iterations,   std::numeric_limits<int>::max(   ),   max_planning_time,   401,   size_theta );
           std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
           std::make_unique<nav2_smac_planner::GridCollisionChecker>(  costmap,   size_theta );
           checker->setFootprint(  nav2_costmap_2d::Footprint(   ),   true,   0.0 );
          
           // Create A* search to smooth
           a_star.setCollisionChecker(  checker.get(   ) );
           a_star.setStart(  5u,   5u,   0u );
           a_star.setGoal(  45u,   45u,   36u );
           nav2_smac_planner::NodeHybrid::CoordinateVector path;
           EXPECT_TRUE(  a_star.createPath(  path,   num_it,   tolerance ) );
          
           // Convert to world coordinates and get length to compare to smoothed length
           nav_msgs::msg::Path plan;
           plan.header.stamp = node->now(   );
           plan.header.frame_id = "map";
           geometry_msgs::msg::PoseStamped pose;
           pose.header = plan.header;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
           double initial_length = 0.0;
           double x_m = path[path.size(   ) - 1].x,   y_m = path[path.size(   ) - 1].y;
           plan.poses.reserve(  path.size(   ) );
           for (  int i = path.size(   ) - 1; i >= 0; --i ) {
           pose.pose = nav2_smac_planner::getWorldCoords(  path[i].x,   path[i].y,   costmap );
           pose.pose.orientation = nav2_smac_planner::getWorldOrientation(  path[i].theta );
           plan.poses.push_back(  pose );
           initial_length += hypot(  path[i].x - x_m,   path[i].y - y_m );
           x_m = path[i].x;
           y_m = path[i].y;
           }
          
           // Check that we accurately detect that this path has a reversing segment
           EXPECT_EQ(  smoother->findDirectionalPathSegmentsWrapper(  plan ).size(   ),   2u );
          
           // Test smoother,   should succeed with same number of points
           // and shorter overall length,   while still being collision free.
           auto path_size_in = plan.poses.size(   );
           EXPECT_TRUE(  smoother->smooth(  plan,   costmap,   maxtime ) );
           EXPECT_EQ(  plan.poses.size(   ),   path_size_in ); // Should have same number of poses
           double length = 0.0;
           x_m = plan.poses[0].pose.position.x;
           y_m = plan.poses[0].pose.position.y;
           for (  unsigned int i = 0; i != plan.poses.size(   ); i++ ) {
           // Should be collision free
           EXPECT_EQ(  costmap->getCost(  plan.poses[i].pose.position.x,   plan.poses[i].pose.position.y ),   0 );
           length += hypot(  plan.poses[i].pose.position.x - x_m,   plan.poses[i].pose.position.y - y_m );
           x_m = plan.poses[i].pose.position.x;
           y_m = plan.poses[i].pose.position.y;
           }
           EXPECT_LT(  length,   initial_length ); // Should be shorter
          
           // Try again but with failure modes
          
           // Failure mode: not enough iterations to complete
           params.max_its_ = 0;
           auto smoother_bypass = std::make_unique<SmootherWrapper>(  params );
           EXPECT_FALSE(  smoother_bypass->smooth(  plan,   costmap,   maxtime ) );
           params.max_its_ = 1;
           auto smoother_failure = std::make_unique<SmootherWrapper>(  params );
           EXPECT_FALSE(  smoother_failure->smooth(  plan,   costmap,   maxtime ) );
          
           // Failure mode: Not enough time
           double max_no_time = 0.0;
           EXPECT_FALSE(  smoother->smooth(  plan,   costmap,   max_no_time ) );
          
           // Failure mode: Path is in collision,   do 2x to exercise overlapping point
           // attempts to update orientation should also fail
           pose.pose.position.x = 1.25;
           pose.pose.position.y = 1.25;
           plan.poses.push_back(  pose );
           plan.poses.push_back(  pose );
           EXPECT_FALSE(  smoother->smooth(  plan,   costmap,   maxtime ) );
           EXPECT_NEAR(  plan.poses.end(   )[-2].pose.orientation.z,   1.0,   1e-3 );
           EXPECT_NEAR(  plan.poses.end(   )[-2].pose.orientation.x,   0.0,   1e-3 );
           EXPECT_NEAR(  plan.poses.end(   )[-2].pose.orientation.y,   0.0,   1e-3 );
           EXPECT_NEAR(  plan.poses.end(   )[-2].pose.orientation.w,   0.0,   1e-3 );
          
           delete costmap;
          }

navigation2/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
          #define NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
          
          #include <memory>
          #include <string>
          #include <thread>
          #include <unordered_map>
          #include <vector>
          
          #include "nav2_core/smoother.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
          #include "nav2_costmap_2d/footprint_subscriber.hpp"
          #include "nav2_msgs/action/smooth_path.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "pluginlib/class_loader.hpp"
          
          namespace nav2_smoother
          {
          
          /**
           * @class nav2_smoother::SmootherServer
           * @brief This class hosts variety of plugins of different algorithms to
           * smooth or refine a path from the exposed SmoothPath action server.
           */
      45  class SmootherServer : public nav2_util::LifecycleNode
          {
          public:
           using SmootherMap = std::unordered_map<std::string,   nav2_core::Smoother::Ptr>;
          
           /**
           * @brief A constructor for nav2_smoother::SmootherServer
           * @param options Additional options to control creation of the node.
           */
           explicit SmootherServer(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief Destructor for nav2_smoother::SmootherServer
           */
           ~SmootherServer(   );
          
          protected:
           /**
           * @brief Configures smoother parameters and member variables
           *
           * Configures smoother plugin and costmap; Initialize odom subscriber,  
           * velocity publisher and smooth path action server.
           * @param state LifeCycle Node's state
           * @return Success or Failure
           * @throw pluginlib::PluginlibException When failed to initialize smoother
           * plugin
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Loads smoother plugins from parameter file
           * @return bool if successfully loaded the plugins
           */
           bool loadSmootherPlugins(   );
          
           /**
           * @brief Activates member variables
           *
           * Activates smoother,   costmap,   velocity publisher and smooth path action
           * server
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Deactivates member variables
           *
           * Deactivates smooth path action server,   smoother,   costmap and velocity
           * publisher. Before calling deactivate state,   velocity is being set to zero.
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Calls clean up states and resets member variables.
           *
           * Smoother and costmap clean up state is called,   and resets rest of the
           * variables
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Called when in Shutdown state
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           using Action = nav2_msgs::action::SmoothPath;
           using ActionServer = nav2_util::SimpleActionServer<Action>;
          
           /**
           * @brief SmoothPath action server callback. Handles action server updates and
           * spins server until goal is reached
           *
           * Provides global path to smoother received from action client. Local
           * section of the path is optimized using smoother.
           * @throw nav2_core::PlannerException
           */
           void smoothPlan(   );
          
           /**
           * @brief Find the valid smoother ID name for the given request
           *
           * @param c_name The requested smoother name
           * @param name Reference to the name to use for control if any valid available
           * @return bool Whether it found a valid smoother to use
           */
           bool findSmootherId(  const std::string & c_name,   std::string & name );
          
           // Our action server implements the SmoothPath action
           std::unique_ptr<ActionServer> action_server_;
          
           // Transforms
           std::shared_ptr<tf2_ros::Buffer> tf_;
           std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
          
           // Publishers and subscribers
           rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
          
           // Smoother Plugins
           pluginlib::ClassLoader<nav2_core::Smoother> lp_loader_;
           SmootherMap smoothers_;
           std::vector<std::string> default_ids_;
           std::vector<std::string> default_types_;
           std::vector<std::string> smoother_ids_;
           std::vector<std::string> smoother_types_;
           std::string smoother_ids_concat_,   current_smoother_;
          
           // Utilities
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
           std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
          
           rclcpp::Clock steady_clock_;
          };
          
          } // namespace nav2_smoother
          
          #endif // NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_

navigation2/nav2_smoother/include/nav2_smoother/simple_smoother.hpp

          // Copyright (  c ) 2022,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
          #define NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
          
          #include <cmath>
          #include <vector>
          #include <string>
          #include <iostream>
          #include <memory>
          #include <queue>
          #include <utility>
          
          #include "nav2_core/smoother.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "angles/angles.h"
          #include "tf2/utils.h"
          
          namespace nav2_smoother
          {
          
          /**
           * @class nav2_smoother::PathSegment
           * @brief A segment of a path in start/end indices
           */
          struct PathSegment
          {
           unsigned int start;
           unsigned int end;
          };
          
          typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
          typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
          
          /**
           * @class nav2_smoother::SimpleSmoother
           * @brief A path smoother implementation
           */
      55  class SimpleSmoother : public nav2_core::Smoother
          {
          public:
           /**
           * @brief A constructor for nav2_smoother::SimpleSmoother
           */
           SimpleSmoother(   ) = default;
          
           /**
           * @brief A destructor for nav2_smoother::SimpleSmoother
           */
           ~SimpleSmoother(   ) override = default;
          
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr &,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer>,  
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,  
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) override;
          
           /**
           * @brief Method to cleanup resources.
           */
           void cleanup(   ) override {costmap_sub_.reset(   );}
          
           /**
           * @brief Method to activate smoother and any threads involved in execution.
           */
           void activate(   ) override {}
          
           /**
           * @brief Method to deactivate smoother and any threads involved in execution.
           */
           void deactivate(   ) override {}
          
           /**
           * @brief Method to smooth given path
           *
           * @param path In-out path to be smoothed
           * @param max_time Maximum duration smoothing should take
           * @return If smoothing was completed (  true ) or interrupted by time limit (  false )
           */
           bool smooth(  
           nav_msgs::msg::Path & path,  
           const rclcpp::Duration & max_time ) override;
          
          protected:
           /**
           * @brief Smoother method - does the smoothing on a segment
           * @param path Reference to path
           * @param reversing_segment Return if this is a reversing segment
           * @param costmap Pointer to minimal costmap
           * @param max_time Maximum time to compute,   stop early if over limit
           * @return If smoothing was successful
           */
     109   bool smoothImpl(  
           nav_msgs::msg::Path & path,  
           bool & reversing_segment,  
           const nav2_costmap_2d::Costmap2D * costmap,  
           const double & max_time );
          
           /**
           * @brief Get the field value for a given dimension
           * @param msg Current pose to sample
           * @param dim Dimension ID of interest
           * @return dim value
           */
     121   inline double getFieldByDim(  
           const geometry_msgs::msg::PoseStamped & msg,  
           const unsigned int & dim );
          
           /**
           * @brief Set the field value for a given dimension
           * @param msg Current pose to sample
           * @param dim Dimension ID of interest
           * @param value to set the dimention to for the pose
           */
     131   inline void setFieldByDim(  
           geometry_msgs::msg::PoseStamped & msg,   const unsigned int dim,  
           const double & value );
          
           /**
           * @brief Finds the starting and end indices of path segments where
           * the robot is traveling in the same direction (  e.g. forward vs reverse )
           * @param path Path in which to look for cusps
           * @return Set of index pairs for each segment of the path in a given direction
           */
     141   std::vector<PathSegment> findDirectionalPathSegments(  const nav_msgs::msg::Path & path );
          
           /**
           * @brief For a given path,   update the path point orientations based on smoothing
           * @param path Path to approximate the path orientation in
           * @param reversing_segment Return if this is a reversing segment
           */
     148   inline void updateApproximatePathOrientations(  
           nav_msgs::msg::Path & path,  
           bool & reversing_segment );
          
           double tolerance_,   data_w_,   smooth_w_;
           int max_its_,   refinement_ctr_;
           bool do_refinement_;
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
     156   rclcpp::Logger logger_{rclcpp::get_logger(  "SimpleSmoother" )};
          };
          
          } // namespace nav2_smoother
          
          #endif // NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_

navigation2/nav2_smoother/src/main.cpp

       1  // Copyright (  c ) 2021 RoboTech Vision
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_smoother/nav2_smoother.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      21  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_smoother::SmootherServer>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_smoother/src/nav2_smoother.cpp

          // Copyright (  c ) 2019 RoboTech Vision
          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <limits>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "nav2_core/exceptions.hpp"
          #include "nav2_smoother/nav2_smoother.hpp"
          #include "nav2_util/geometry_utils.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav_2d_utils/conversions.hpp"
          #include "nav_2d_utils/tf_help.hpp"
          #include "tf2_ros/create_timer_ros.h"
          
          using namespace std::chrono_literals;
          
          namespace nav2_smoother
          {
          
      36  SmootherServer::SmootherServer(  const rclcpp::NodeOptions & options )
          : LifecycleNode(  "smoother_server",   "",   options ),  
           lp_loader_(  "nav2_core",   "nav2_core::Smoother" ),  
           default_ids_{"simple_smoother"},  
      40   default_types_{"nav2_smoother::SimpleSmoother"}
          {
      42   RCLCPP_INFO(  get_logger(   ),   "Creating smoother server" );
          
      44   declare_parameter(  
           "costmap_topic",   rclcpp::ParameterValue(  
           std::string(  
           "global_costmap/costmap_raw" ) ) );
      48   declare_parameter(  
           "footprint_topic",  
           rclcpp::ParameterValue(  
           std::string(  "global_costmap/published_footprint" ) ) );
      52   declare_parameter(  
           "robot_base_frame",  
           rclcpp::ParameterValue(  std::string(  "base_link" ) ) );
      55   declare_parameter(  "transform_tolerance",   rclcpp::ParameterValue(  0.1 ) );
      56   declare_parameter(  "smoother_plugins",   default_ids_ );
          }
          
          SmootherServer::~SmootherServer(   )
          {
           smoothers_.clear(   );
          }
          
          nav2_util::CallbackReturn
          SmootherServer::on_configure(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring smoother server" );
          
           auto node = shared_from_this(   );
          
           get_parameter(  "smoother_plugins",   smoother_ids_ );
           if (  smoother_ids_ == default_ids_ ) {
           for (  size_t i = 0; i < default_ids_.size(   ); ++i ) {
           nav2_util::declare_parameter_if_not_declared(  
           node,   default_ids_[i] + ".plugin",  
           rclcpp::ParameterValue(  default_types_[i] ) );
           }
           }
          
           tf_ = std::make_shared<tf2_ros::Buffer>(  get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           get_node_base_interface(   ),   get_node_timers_interface(   ) );
           tf_->setCreateTimerInterface(  timer_interface );
           transform_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_ );
          
           std::string costmap_topic,   footprint_topic,   robot_base_frame;
           double transform_tolerance;
           this->get_parameter(  "costmap_topic",   costmap_topic );
           this->get_parameter(  "footprint_topic",   footprint_topic );
           this->get_parameter(  "transform_tolerance",   transform_tolerance );
           this->get_parameter(  "robot_base_frame",   robot_base_frame );
           costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(  
           shared_from_this(   ),   costmap_topic );
           footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(  
           shared_from_this(   ),   footprint_topic,   *tf_,   robot_base_frame,   transform_tolerance );
          
           collision_checker_ =
           std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(  
           *costmap_sub_,   *footprint_sub_,   this->get_name(   ) );
          
           if (  !loadSmootherPlugins(   ) ) {
           return nav2_util::CallbackReturn::FAILURE;
           }
          
           // Initialize pubs & subs
           plan_publisher_ = create_publisher<nav_msgs::msg::Path>(  "plan_smoothed",   1 );
          
           // Create the action server that we implement with our smoothPath method
           action_server_ = std::make_unique<ActionServer>(  
           shared_from_this(   ),  
           "smooth_path",  
           std::bind(  &SmootherServer::smoothPlan,   this ),  
           nullptr,  
           std::chrono::milliseconds(  500 ),  
           true );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          bool SmootherServer::loadSmootherPlugins(   )
          {
           auto node = shared_from_this(   );
          
           smoother_types_.resize(  smoother_ids_.size(   ) );
          
           for (  size_t i = 0; i != smoother_ids_.size(   ); i++ ) {
           try {
           smoother_types_[i] =
           nav2_util::get_plugin_type_param(  node,   smoother_ids_[i] );
           nav2_core::Smoother::Ptr smoother =
           lp_loader_.createUniqueInstance(  smoother_types_[i] );
           RCLCPP_INFO(  
           get_logger(   ),   "Created smoother : %s of type %s",  
           smoother_ids_[i].c_str(   ),   smoother_types_[i].c_str(   ) );
           smoother->configure(  
           node,   smoother_ids_[i],   tf_,   costmap_sub_,  
           footprint_sub_ );
           smoothers_.insert(  {smoother_ids_[i],   smoother} );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),   "Failed to create smoother. Exception: %s",  
           ex.what(   ) );
           return false;
           }
           }
          
           for (  size_t i = 0; i != smoother_ids_.size(   ); i++ ) {
           smoother_ids_concat_ += smoother_ids_[i] + std::string(  " " );
           }
          
           RCLCPP_INFO(  
           get_logger(   ),   "Smoother Server has %s smoothers available.",  
           smoother_ids_concat_.c_str(   ) );
          
           return true;
          }
          
          nav2_util::CallbackReturn
          SmootherServer::on_activate(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           plan_publisher_->on_activate(   );
           SmootherMap::iterator it;
           for (  it = smoothers_.begin(   ); it != smoothers_.end(   ); ++it ) {
           it->second->activate(   );
           }
           action_server_->activate(   );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          SmootherServer::on_deactivate(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           action_server_->deactivate(   );
           SmootherMap::iterator it;
           for (  it = smoothers_.begin(   ); it != smoothers_.end(   ); ++it ) {
           it->second->deactivate(   );
           }
           plan_publisher_->on_deactivate(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          SmootherServer::on_cleanup(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           // Cleanup the helper classes
           SmootherMap::iterator it;
           for (  it = smoothers_.begin(   ); it != smoothers_.end(   ); ++it ) {
           it->second->cleanup(   );
           }
           smoothers_.clear(   );
          
           // Release any allocated resources
           action_server_.reset(   );
           plan_publisher_.reset(   );
           transform_listener_.reset(   );
           tf_.reset(   );
           footprint_sub_.reset(   );
           costmap_sub_.reset(   );
           collision_checker_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          SmootherServer::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          bool SmootherServer::findSmootherId(  
           const std::string & c_name,  
           std::string & current_smoother )
          {
           if (  smoothers_.find(  c_name ) == smoothers_.end(   ) ) {
           if (  smoothers_.size(   ) == 1 && c_name.empty(   ) ) {
           RCLCPP_WARN_ONCE(  
           get_logger(   ),  
           "No smoother was specified in action call."
           " Server will use only plugin loaded %s. "
           "This warning will appear once.",  
           smoother_ids_concat_.c_str(   ) );
           current_smoother = smoothers_.begin(   )->first;
           } else {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "SmoothPath called with smoother name %s,   "
           "which does not exist. Available smoothers are: %s.",  
           c_name.c_str(   ),   smoother_ids_concat_.c_str(   ) );
           return false;
           }
           } else {
           RCLCPP_DEBUG(  get_logger(   ),   "Selected smoother: %s.",   c_name.c_str(   ) );
           current_smoother = c_name;
           }
          
           return true;
          }
          
          void SmootherServer::smoothPlan(   )
          {
           auto start_time = steady_clock_.now(   );
          
           RCLCPP_INFO(  get_logger(   ),   "Received a path to smooth." );
          
           auto result = std::make_shared<Action::Result>(   );
           try {
           std::string c_name = action_server_->get_current_goal(   )->smoother_id;
           std::string current_smoother;
           if (  findSmootherId(  c_name,   current_smoother ) ) {
           current_smoother_ = current_smoother;
           } else {
           action_server_->terminate_current(   );
           return;
           }
          
           // Perform smoothing
           auto goal = action_server_->get_current_goal(   );
           result->path = goal->path;
           result->was_completed = smoothers_[current_smoother_]->smooth(  
           result->path,   goal->max_smoothing_duration );
           result->smoothing_duration = steady_clock_.now(   ) - start_time;
          
           if (  !result->was_completed ) {
           RCLCPP_INFO(  
           get_logger(   ),  
           "Smoother %s did not complete smoothing in specified time limit"
           "(  %lf seconds ) and was interrupted after %lf seconds",  
           current_smoother_.c_str(   ),  
           rclcpp::Duration(  goal->max_smoothing_duration ).seconds(   ),  
           rclcpp::Duration(  result->smoothing_duration ).seconds(   ) );
           }
           plan_publisher_->publish(  result->path );
          
           // Check for collisions
           if (  goal->check_for_collisions ) {
           geometry_msgs::msg::Pose2D pose2d;
           bool fetch_data = true;
           for (  const auto & pose : result->path.poses ) {
           pose2d.x = pose.pose.position.x;
           pose2d.y = pose.pose.position.y;
           pose2d.theta = tf2::getYaw(  pose.pose.orientation );
          
           if (  !collision_checker_->isCollisionFree(  pose2d,   fetch_data ) ) {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Smoothed path leads to a collision at x: %lf,   y: %lf,   theta: %lf",  
           pose2d.x,   pose2d.y,   pose2d.theta );
           action_server_->terminate_current(  result );
           return;
           }
           fetch_data = false;
           }
           }
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "Smoother succeeded (  time: %lf ),   setting result",  
           rclcpp::Duration(  result->smoothing_duration ).seconds(   ) );
          
           action_server_->succeeded_current(  result );
           } catch (  nav2_core::PlannerException & e ) {
           RCLCPP_ERROR(  this->get_logger(   ),   e.what(   ) );
           action_server_->terminate_current(   );
           return;
           }
          }
          
          } // namespace nav2_smoother
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
          RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_smoother::SmootherServer )

navigation2/nav2_smoother/src/simple_smoother.cpp

       1  // Copyright (  c ) 2022,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <vector>
          #include <memory>
          #include "nav2_smoother/simple_smoother.hpp"
          
          namespace nav2_smoother
          {
          using namespace nav2_util::geometry_utils; // NOLINT
          using namespace std::chrono; // NOLINT
          using nav2_util::declare_parameter_if_not_declared;
          
      25  void SimpleSmoother::configure(  
      26   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      27   std::string name,   std::shared_ptr<tf2_ros::Buffer>/*tf*/,  
      28   std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,  
      29   std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>/*footprint_sub*/ )
          {
           costmap_sub_ = costmap_sub;
          
           auto node = parent.lock(   );
           logger_ = node->get_logger(   );
          
           declare_parameter_if_not_declared(  
           node,   name + ".tolerance",   rclcpp::ParameterValue(  1e-10 ) );
           declare_parameter_if_not_declared(  
           node,   name + ".max_its",   rclcpp::ParameterValue(  1000 ) );
           declare_parameter_if_not_declared(  
           node,   name + ".w_data",   rclcpp::ParameterValue(  0.2 ) );
           declare_parameter_if_not_declared(  
           node,   name + ".w_smooth",   rclcpp::ParameterValue(  0.3 ) );
           declare_parameter_if_not_declared(  
           node,   name + ".do_refinement",   rclcpp::ParameterValue(  true ) );
          
           node->get_parameter(  name + ".tolerance",   tolerance_ );
           node->get_parameter(  name + ".max_its",   max_its_ );
           node->get_parameter(  name + ".w_data",   data_w_ );
           node->get_parameter(  name + ".w_smooth",   smooth_w_ );
           node->get_parameter(  name + ".do_refinement",   do_refinement_ );
          }
          
      54  bool SimpleSmoother::smooth(  
      55   nav_msgs::msg::Path & path,  
      56   const rclcpp::Duration & max_time )
          {
           auto costmap = costmap_sub_->getCostmap(   );
          
           refinement_ctr_ = 0;
           steady_clock::time_point start = steady_clock::now(   );
           double time_remaining = max_time.seconds(   );
          
           bool success = true,   reversing_segment;
           nav_msgs::msg::Path curr_path_segment;
           curr_path_segment.header = path.header;
          
           std::vector<PathSegment> path_segments = findDirectionalPathSegments(  path );
          
           for (  unsigned int i = 0; i != path_segments.size(   ); i++ ) {
           if (  path_segments[i].end - path_segments[i].start > 9 ) {
           // Populate path segment
           curr_path_segment.poses.clear(   );
           std::copy(  
           path.poses.begin(   ) + path_segments[i].start,  
           path.poses.begin(   ) + path_segments[i].end + 1,  
           std::back_inserter(  curr_path_segment.poses ) );
          
           // Make sure we're still able to smooth with time remaining
           steady_clock::time_point now = steady_clock::now(   );
           time_remaining = max_time.seconds(   ) - duration_cast<duration<double>>(  now - start ).count(   );
          
           // Smooth path segment naively
           success = success && smoothImpl(  
           curr_path_segment,   reversing_segment,   costmap.get(   ),   time_remaining );
          
           // Assemble the path changes to the main path
           std::copy(  
           curr_path_segment.poses.begin(   ),  
           curr_path_segment.poses.end(   ),  
           path.poses.begin(   ) + path_segments[i].start );
           }
           }
          
           return success;
          }
          
      98  bool SimpleSmoother::smoothImpl(  
      99   nav_msgs::msg::Path & path,  
     100   bool & reversing_segment,  
     101   const nav2_costmap_2d::Costmap2D * costmap,  
           const double & max_time )
          {
           steady_clock::time_point a = steady_clock::now(   );
           rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(  max_time );
          
           int its = 0;
           double change = tolerance_;
           const unsigned int & path_size = path.poses.size(   );
           double x_i,   y_i,   y_m1,   y_ip1,   y_i_org;
           unsigned int mx,   my;
          
           nav_msgs::msg::Path new_path = path;
           nav_msgs::msg::Path last_path = path;
          
           while (  change >= tolerance_ ) {
           its += 1;
           change = 0.0;
          
           // Make sure the smoothing function will converge
           if (  its >= max_its_ ) {
           RCLCPP_WARN(  
           logger_,  
           "Number of iterations has exceeded limit of %i.",   max_its_ );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
          
           // Make sure still have time left to process
           steady_clock::time_point b = steady_clock::now(   );
           rclcpp::Duration timespan(  duration_cast<duration<double>>(  b - a ) );
           if (  timespan > max_dur ) {
           RCLCPP_WARN(  
           logger_,  
           "Smoothing time exceeded allowed duration of %0.2f.",   max_time );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
          
           for (  unsigned int i = 1; i != path_size - 1; i++ ) {
           for (  unsigned int j = 0; j != 2; j++ ) {
           x_i = getFieldByDim(  path.poses[i],   j );
           y_i = getFieldByDim(  new_path.poses[i],   j );
           y_m1 = getFieldByDim(  new_path.poses[i - 1],   j );
           y_ip1 = getFieldByDim(  new_path.poses[i + 1],   j );
           y_i_org = y_i;
          
           // Smooth based on local 3 point neighborhood and original data locations
           y_i += data_w_ * (  x_i - y_i ) + smooth_w_ * (  y_ip1 + y_m1 - (  2.0 * y_i ) );
           setFieldByDim(  new_path.poses[i],   j,   y_i );
           change += abs(  y_i - y_i_org );
           }
          
           // validate update is admissible,   only checks cost if a valid costmap pointer is provided
           float cost = 0.0;
           if (  costmap ) {
           costmap->worldToMap(  
           getFieldByDim(  new_path.poses[i],   0 ),  
           getFieldByDim(  new_path.poses[i],   1 ),  
           mx,   my );
           cost = static_cast<float>(  costmap->getCost(  mx,   my ) );
           }
          
           if (  cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION ) {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "SmacPlannerSmoother" ),  
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced." );
           path = last_path;
           updateApproximatePathOrientations(  path,   reversing_segment );
           return false;
           }
           }
          
           last_path = new_path;
           }
          
           // Lets do additional refinement,   it shouldn't take more than a couple milliseconds
           // but really puts the path quality over the top.
           if (  do_refinement_ && refinement_ctr_ < 4 ) {
           refinement_ctr_++;
           smoothImpl(  new_path,   reversing_segment,   costmap,   max_time );
           }
          
           updateApproximatePathOrientations(  new_path,   reversing_segment );
           path = new_path;
           return true;
          }
          
     192  double SimpleSmoother::getFieldByDim(  
     193   const geometry_msgs::msg::PoseStamped & msg,   const unsigned int & dim )
          {
           if (  dim == 0 ) {
           return msg.pose.position.x;
           } else if (  dim == 1 ) {
           return msg.pose.position.y;
           } else {
           return msg.pose.position.z;
           }
          }
          
     204  void SimpleSmoother::setFieldByDim(  
     205   geometry_msgs::msg::PoseStamped & msg,   const unsigned int dim,  
           const double & value )
          {
           if (  dim == 0 ) {
           msg.pose.position.x = value;
           } else if (  dim == 1 ) {
           msg.pose.position.y = value;
           } else {
           msg.pose.position.z = value;
           }
          }
          
     217  std::vector<PathSegment> SimpleSmoother::findDirectionalPathSegments(  
     218   const nav_msgs::msg::Path & path )
          {
           std::vector<PathSegment> segments;
           PathSegment curr_segment;
           curr_segment.start = 0;
          
           // Iterating through the path to determine the position of the cusp
           for (  unsigned int idx = 1; idx < path.poses.size(   ) - 1; ++idx ) {
           // We have two vectors for the dot product OA and AB. Determining the vectors.
           double oa_x = path.poses[idx].pose.position.x -
           path.poses[idx - 1].pose.position.x;
           double oa_y = path.poses[idx].pose.position.y -
           path.poses[idx - 1].pose.position.y;
           double ab_x = path.poses[idx + 1].pose.position.x -
           path.poses[idx].pose.position.x;
           double ab_y = path.poses[idx + 1].pose.position.y -
           path.poses[idx].pose.position.y;
          
           // Checking for the existance of cusp,   in the path,   using the dot product.
           double dot_product = (  oa_x * ab_x ) + (  oa_y * ab_y );
           if (  dot_product < 0.0 ) {
           curr_segment.end = idx;
           segments.push_back(  curr_segment );
           curr_segment.start = idx;
           }
          
           // Checking for the existance of a differential rotation in place.
           double cur_theta = tf2::getYaw(  path.poses[idx].pose.orientation );
           double next_theta = tf2::getYaw(  path.poses[idx + 1].pose.orientation );
           double dtheta = angles::shortest_angular_distance(  cur_theta,   next_theta );
           if (  fabs(  ab_x ) < 1e-4 && fabs(  ab_y ) < 1e-4 && fabs(  dtheta ) > 1e-4 ) {
           curr_segment.end = idx;
           segments.push_back(  curr_segment );
           curr_segment.start = idx;
           }
           }
          
           curr_segment.end = path.poses.size(   ) - 1;
           segments.push_back(  curr_segment );
           return segments;
          }
          
     260  void SimpleSmoother::updateApproximatePathOrientations(  
     261   nav_msgs::msg::Path & path,  
     262   bool & reversing_segment )
          {
           double dx,   dy,   theta,   pt_yaw;
           reversing_segment = false;
          
           // Find if this path segment is in reverse
           dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
           dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
           theta = atan2(  dy,   dx );
           pt_yaw = tf2::getYaw(  path.poses[1].pose.orientation );
           if (  fabs(  angles::shortest_angular_distance(  pt_yaw,   theta ) ) > M_PI_2 ) {
           reversing_segment = true;
           }
          
           // Find the angle relative the path position vectors
           for (  unsigned int i = 0; i != path.poses.size(   ) - 1; i++ ) {
           dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
           dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
           theta = atan2(  dy,   dx );
          
           // If points are overlapping,   pass
           if (  fabs(  dx ) < 1e-4 && fabs(  dy ) < 1e-4 ) {
           continue;
           }
          
           // Flip the angle if this path segment is in reverse
           if (  reversing_segment ) {
           theta += M_PI; // orientationAroundZAxis will normalize
           }
          
           path.poses[i].pose.orientation = orientationAroundZAxis(  theta );
           }
          }
          
          } // namespace nav2_smoother
          
          #include "pluginlib/class_list_macros.hpp"
     299  PLUGINLIB_EXPORT_CLASS(  nav2_smoother::SimpleSmoother,   nav2_core::Smoother )

navigation2/nav2_smoother/test/test_simple_smoother.cpp

       1  // Copyright (  c ) 2022,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <chrono>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d.hpp"
          #include "nav2_costmap_2d/costmap_subscriber.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_smoother/simple_smoother.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          using namespace nav2_smoother; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      34  class RclCppFixture
          {
          public:
      37   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      38   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      42  class SmootherWrapper : public nav2_smoother::SimpleSmoother
          {
          public:
      45   SmootherWrapper(   )
           : nav2_smoother::SimpleSmoother(   )
           {
           }
          
      50   std::vector<PathSegment> findDirectionalPathSegmentsWrapper(  nav_msgs::msg::Path path )
           {
           return findDirectionalPathSegments(  path );
           }
          
      55   void setMaxItsToInvalid(   )
           {
           max_its_ = 0;
           }
          };
          
      61  TEST(  SmootherTest,   test_simple_smoother )
          {
           rclcpp_lifecycle::LifecycleNode::SharedPtr node =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "SmacSmootherTest" );
          
           std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
           std::make_shared<nav2_msgs::msg::Costmap>(   );
           costmap_msg->header.stamp = node->now(   );
           costmap_msg->header.frame_id = "map";
           costmap_msg->data.resize(  100 * 100 );
           costmap_msg->metadata.resolution = 0.05;
           costmap_msg->metadata.size_x = 100;
           costmap_msg->metadata.size_y = 100;
          
           // island in the middle of lethal cost to cross
           for (  unsigned int i = 20; i <= 30; ++i ) {
           for (  unsigned int j = 20; j <= 30; ++j ) {
           costmap_msg->data[j * 100 + i] = 254;
           }
           }
          
           std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
           std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
           dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(  parent,   "dummy_topic" );
           dummy_costmap->costmapCallback(  costmap_msg );
          
           // Make smoother
           std::shared_ptr<tf2_ros::Buffer> dummy_tf;
           std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
           auto smoother = std::make_unique<SmootherWrapper>(   );
           smoother->configure(  parent,   "test",   dummy_tf,   dummy_costmap,   dummy_footprint );
          
           // Test that an irregular distributed path becomes more distributed
           nav_msgs::msg::Path straight_irregular_path;
           straight_irregular_path.header.frame_id = "map";
           straight_irregular_path.header.stamp = node->now(   );
           straight_irregular_path.poses.resize(  11 );
           straight_irregular_path.poses[0].pose.position.x = 0.5;
           straight_irregular_path.poses[0].pose.position.y = 0.0;
           straight_irregular_path.poses[1].pose.position.x = 0.5;
           straight_irregular_path.poses[1].pose.position.y = 0.1;
           straight_irregular_path.poses[2].pose.position.x = 0.5;
           straight_irregular_path.poses[2].pose.position.y = 0.2;
           straight_irregular_path.poses[3].pose.position.x = 0.5;
           straight_irregular_path.poses[3].pose.position.y = 0.35;
           straight_irregular_path.poses[4].pose.position.x = 0.5;
           straight_irregular_path.poses[4].pose.position.y = 0.4;
           straight_irregular_path.poses[5].pose.position.x = 0.5;
           straight_irregular_path.poses[5].pose.position.y = 0.56;
           straight_irregular_path.poses[6].pose.position.x = 0.5;
           straight_irregular_path.poses[6].pose.position.y = 0.9;
           straight_irregular_path.poses[7].pose.position.x = 0.5;
           straight_irregular_path.poses[7].pose.position.y = 0.95;
           straight_irregular_path.poses[8].pose.position.x = 0.5;
           straight_irregular_path.poses[8].pose.position.y = 1.3;
           straight_irregular_path.poses[9].pose.position.x = 0.5;
           straight_irregular_path.poses[9].pose.position.y = 2.0;
           straight_irregular_path.poses[10].pose.position.x = 0.5;
           straight_irregular_path.poses[10].pose.position.y = 2.5;
          
           rclcpp::Duration no_time = rclcpp::Duration::from_seconds(  0.0 ); // 0 seconds
           rclcpp::Duration max_time = rclcpp::Duration::from_seconds(  1 ); // 1 second
           EXPECT_FALSE(  smoother->smooth(  straight_irregular_path,   no_time ) );
           EXPECT_TRUE(  smoother->smooth(  straight_irregular_path,   max_time ) );
           for (  uint i = 0; i != straight_irregular_path.poses.size(   ) - 1; i++ ) {
           // Check distances are more evenly spaced out now
           EXPECT_LT(  
           fabs(  
           straight_irregular_path.poses[i].pose.position.y -
           straight_irregular_path.poses[i + 1].pose.position.y ),   0.38 );
           }
          
           // Test regular path,   should see no effective change
           nav_msgs::msg::Path straight_regular_path;
           straight_regular_path.header = straight_irregular_path.header;
           straight_regular_path.poses.resize(  11 );
           straight_regular_path.poses[0].pose.position.x = 0.5;
           straight_regular_path.poses[0].pose.position.y = 0.0;
           straight_regular_path.poses[1].pose.position.x = 0.5;
           straight_regular_path.poses[1].pose.position.y = 0.1;
           straight_regular_path.poses[2].pose.position.x = 0.5;
           straight_regular_path.poses[2].pose.position.y = 0.2;
           straight_regular_path.poses[3].pose.position.x = 0.5;
           straight_regular_path.poses[3].pose.position.y = 0.3;
           straight_regular_path.poses[4].pose.position.x = 0.5;
           straight_regular_path.poses[4].pose.position.y = 0.4;
           straight_regular_path.poses[5].pose.position.x = 0.5;
           straight_regular_path.poses[5].pose.position.y = 0.5;
           straight_regular_path.poses[6].pose.position.x = 0.5;
           straight_regular_path.poses[6].pose.position.y = 0.6;
           straight_regular_path.poses[7].pose.position.x = 0.5;
           straight_regular_path.poses[7].pose.position.y = 0.7;
           straight_regular_path.poses[8].pose.position.x = 0.5;
           straight_regular_path.poses[8].pose.position.y = 0.8;
           straight_regular_path.poses[9].pose.position.x = 0.5;
           straight_regular_path.poses[9].pose.position.y = 0.9;
           straight_regular_path.poses[10].pose.position.x = 0.5;
           straight_regular_path.poses[10].pose.position.y = 1.0;
          
           EXPECT_TRUE(  smoother->smooth(  straight_regular_path,   max_time ) );
           for (  uint i = 0; i != straight_regular_path.poses.size(   ) - 1; i++ ) {
           // Check distances are still very evenly spaced
           EXPECT_NEAR(  
           fabs(  
           straight_regular_path.poses[i].pose.position.y -
           straight_regular_path.poses[i + 1].pose.position.y ),   0.1,   0.001 );
           }
          
           // test shorter and curved if given a right angle
           nav_msgs::msg::Path right_angle_path;
           right_angle_path = straight_regular_path;
           straight_regular_path.poses[6].pose.position.x = 0.6;
           straight_regular_path.poses[6].pose.position.y = 0.5;
           straight_regular_path.poses[7].pose.position.x = 0.7;
           straight_regular_path.poses[7].pose.position.y = 0.5;
           straight_regular_path.poses[8].pose.position.x = 0.8;
           straight_regular_path.poses[8].pose.position.y = 0.5;
           straight_regular_path.poses[9].pose.position.x = 0.9;
           straight_regular_path.poses[9].pose.position.y = 0.5;
           straight_regular_path.poses[10].pose.position.x = 0.95;
           straight_regular_path.poses[10].pose.position.y = 0.5;
           EXPECT_TRUE(  smoother->smooth(  straight_regular_path,   max_time ) );
           EXPECT_NEAR(  straight_regular_path.poses[5].pose.position.x,   0.637,   0.01 );
           EXPECT_NEAR(  straight_regular_path.poses[5].pose.position.y,   0.353,   0.01 );
          
           // Test that collisions are rejected
           nav_msgs::msg::Path collision_path;
           collision_path.poses.resize(  11 );
           collision_path.poses[0].pose.position.x = 0.0;
           collision_path.poses[0].pose.position.y = 0.0;
           collision_path.poses[1].pose.position.x = 0.2;
           collision_path.poses[1].pose.position.y = 0.2;
           collision_path.poses[2].pose.position.x = 0.4;
           collision_path.poses[2].pose.position.y = 0.4;
           collision_path.poses[3].pose.position.x = 0.6;
           collision_path.poses[3].pose.position.y = 0.6;
           collision_path.poses[4].pose.position.x = 0.8;
           collision_path.poses[4].pose.position.y = 0.8;
           collision_path.poses[5].pose.position.x = 1.0;
           collision_path.poses[5].pose.position.y = 1.0;
           collision_path.poses[6].pose.position.x = 1.1;
           collision_path.poses[6].pose.position.y = 1.1;
           collision_path.poses[7].pose.position.x = 1.2;
           collision_path.poses[7].pose.position.y = 1.2;
           collision_path.poses[8].pose.position.x = 1.3;
           collision_path.poses[8].pose.position.y = 1.3;
           collision_path.poses[9].pose.position.x = 1.4;
           collision_path.poses[9].pose.position.y = 1.4;
           collision_path.poses[10].pose.position.x = 1.5;
           collision_path.poses[10].pose.position.y = 1.5;
           EXPECT_FALSE(  smoother->smooth(  collision_path,   max_time ) );
          
           // test cusp / reversing segments
           nav_msgs::msg::Path reversing_path;
           reversing_path.poses.resize(  11 );
           reversing_path.poses[0].pose.position.x = 0.5;
           reversing_path.poses[0].pose.position.y = 0.0;
           reversing_path.poses[1].pose.position.x = 0.5;
           reversing_path.poses[1].pose.position.y = 0.1;
           reversing_path.poses[2].pose.position.x = 0.5;
           reversing_path.poses[2].pose.position.y = 0.2;
           reversing_path.poses[3].pose.position.x = 0.5;
           reversing_path.poses[3].pose.position.y = 0.3;
           reversing_path.poses[4].pose.position.x = 0.5;
           reversing_path.poses[4].pose.position.y = 0.4;
           reversing_path.poses[5].pose.position.x = 0.5;
           reversing_path.poses[5].pose.position.y = 0.5;
           reversing_path.poses[6].pose.position.x = 0.5;
           reversing_path.poses[6].pose.position.y = 0.4;
           reversing_path.poses[7].pose.position.x = 0.5;
           reversing_path.poses[7].pose.position.y = 0.3;
           reversing_path.poses[8].pose.position.x = 0.5;
           reversing_path.poses[8].pose.position.y = 0.2;
           reversing_path.poses[9].pose.position.x = 0.5;
           reversing_path.poses[9].pose.position.y = 0.1;
           reversing_path.poses[10].pose.position.x = 0.5;
           reversing_path.poses[10].pose.position.y = 0.0;
           EXPECT_TRUE(  smoother->smooth(  reversing_path,   max_time ) );
          
           // // test rotate in place
           tf2::Quaternion quat1,   quat2;
           quat1.setRPY(  0.0,   0.0,   0.0 );
           quat2.setRPY(  0.0,   0.0,   1.0 );
           straight_irregular_path.poses[5].pose.position.x = 0.5;
           straight_irregular_path.poses[5].pose.position.y = 0.5;
           straight_irregular_path.poses[5].pose.orientation = tf2::toMsg(  quat1 );
           straight_irregular_path.poses[6].pose.position.x = 0.5;
           straight_irregular_path.poses[6].pose.position.y = 0.5;
           straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(  quat2 );
           EXPECT_TRUE(  smoother->smooth(  straight_irregular_path,   max_time ) );
          
           // test max iterations
           smoother->setMaxItsToInvalid(   );
           nav_msgs::msg::Path max_its_path;
           max_its_path.poses.resize(  11 );
           max_its_path.poses[0].pose.position.x = 0.5;
           max_its_path.poses[0].pose.position.y = 0.0;
           max_its_path.poses[1].pose.position.x = 0.5;
           max_its_path.poses[1].pose.position.y = 0.1;
           max_its_path.poses[2].pose.position.x = 0.5;
           max_its_path.poses[2].pose.position.y = 0.2;
           max_its_path.poses[3].pose.position.x = 0.5;
           max_its_path.poses[3].pose.position.y = 0.3;
           max_its_path.poses[4].pose.position.x = 0.5;
           max_its_path.poses[4].pose.position.y = 0.4;
           max_its_path.poses[5].pose.position.x = 0.5;
           max_its_path.poses[5].pose.position.y = 0.5;
           max_its_path.poses[6].pose.position.x = 0.5;
           max_its_path.poses[6].pose.position.y = 0.6;
           max_its_path.poses[7].pose.position.x = 0.5;
           max_its_path.poses[7].pose.position.y = 0.7;
           max_its_path.poses[8].pose.position.x = 0.5;
           max_its_path.poses[8].pose.position.y = 0.8;
           max_its_path.poses[9].pose.position.x = 0.5;
           max_its_path.poses[9].pose.position.y = 0.9;
           max_its_path.poses[10].pose.position.x = 0.5;
           max_its_path.poses[10].pose.position.y = 1.0;
           EXPECT_FALSE(  smoother->smooth(  max_its_path,   max_time ) );
          }

navigation2/nav2_smoother/test/test_smoother_server.cpp

          // Copyright (  c ) 2021 RoboTech Vision
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <memory>
          #include <chrono>
          #include <iostream>
          #include <future>
          #include <thread>
          #include <algorithm>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_core/smoother.hpp"
          #include "nav2_core/exceptions.hpp"
          #include "nav2_msgs/action/smooth_path.hpp"
          #include "nav2_smoother/nav2_smoother.hpp"
          #include "tf2_ros/create_timer_ros.h"
          
          using SmoothAction = nav2_msgs::action::SmoothPath;
          using ClientGoalHandle = rclcpp_action::ClientGoalHandle<SmoothAction>;
          
          using namespace std::chrono_literals;
          
          // A smoother for testing the base class
          
      41  class DummySmoother : public nav2_core::Smoother
          {
          public:
      44   DummySmoother(   )
           : initialized_(  false ) {}
          
      47   ~DummySmoother(   ) {}
          
      49   virtual void configure(  
      50   const rclcpp_lifecycle::LifecycleNode::WeakPtr &,  
      51   std::string,   std::shared_ptr<tf2_ros::Buffer>,  
      52   std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,  
      53   std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) {}
          
      55   virtual void cleanup(   ) {}
          
      57   virtual void activate(   ) {}
          
      59   virtual void deactivate(   ) {}
          
      61   virtual bool smooth(  
      62   nav_msgs::msg::Path & path,  
      63   const rclcpp::Duration & max_time )
           {
           assert(  path.poses.size(   ) == 2 );
          
           if (  path.poses.front(   ) == path.poses.back(   ) ) {
           throw nav2_core::PlannerException(  "Start and goal pose must differ" );
           }
          
           auto max_time_ms = max_time.to_chrono<std::chrono::milliseconds>(   );
           std::this_thread::sleep_for(  std::min(  max_time_ms,   100ms ) );
          
           // place dummy pose in the middle of the path
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x =
           (  path.poses.front(   ).pose.position.x + path.poses.back(   ).pose.position.x ) / 2;
           pose.pose.position.y =
           (  path.poses.front(   ).pose.position.y + path.poses.back(   ).pose.position.y ) / 2;
           pose.pose.orientation.w = 1.0;
           path.poses.push_back(  pose );
          
           return max_time_ms > 100ms;
           }
          
          private:
      87   bool initialized_;
      88   std::string command_;
      89   std::chrono::system_clock::time_point start_time_;
          };
          
          // Mocked class loader
      93  void onPluginDeletion(  nav2_core::Smoother * obj )
          {
           if (  nullptr != obj ) {
           delete (  obj );
           }
          }
          
          template<>
     101  pluginlib::UniquePtr<nav2_core::Smoother> pluginlib::ClassLoader<nav2_core::Smoother>::
     102  createUniqueInstance(  const std::string & lookup_name )
          {
           if (  lookup_name != "DummySmoother" ) {
           // original method body
           if (  !isClassLoaded(  lookup_name ) ) {
           loadLibraryForClass(  lookup_name );
           }
           try {
           std::string class_type = getClassType(  lookup_name );
           pluginlib::UniquePtr<nav2_core::Smoother> obj =
           lowlevel_class_loader_.createUniqueInstance<nav2_core::Smoother>(  class_type );
           return obj;
           } catch (  const class_loader::CreateClassException & ex ) {
           throw pluginlib::CreateClassException(  ex.what(   ) );
           }
           }
          
           // mocked plugin creation
           return std::unique_ptr<nav2_core::Smoother,  
           class_loader::ClassLoader::DeleterType<nav2_core::Smoother>>(  
           new DummySmoother(   ),  
           onPluginDeletion );
          }
          
     126  class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
          {
          public:
     129   DummyCostmapSubscriber(  
     130   nav2_util::LifecycleNode::SharedPtr node,  
     131   const std::string & topic_name )
           : CostmapSubscriber(  node,   topic_name )
           {
           auto costmap = std::make_shared<nav2_msgs::msg::Costmap>(   );
           costmap->metadata.size_x = 100;
           costmap->metadata.size_y = 100;
           costmap->metadata.resolution = 0.1;
           costmap->metadata.origin.position.x = -5.0;
           costmap->metadata.origin.position.y = -5.0;
          
           costmap->data.resize(  costmap->metadata.size_x * costmap->metadata.size_y,   0 );
           for (  unsigned int i = 0; i < costmap->metadata.size_y; ++i ) {
           for (  unsigned int j = 20; j < 40; ++j ) {
           costmap->data[i * costmap->metadata.size_x + j] = 254;
           }
           }
          
           setCostmap(  costmap );
           }
          
     151   void setCostmap(  nav2_msgs::msg::Costmap::SharedPtr msg )
           {
           costmap_msg_ = msg;
           costmap_received_ = true;
           }
          };
          
     158  class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
          {
          public:
     161   DummyFootprintSubscriber(  
     162   nav2_util::LifecycleNode::SharedPtr node,  
     163   const std::string & topic_name,  
     164   tf2_ros::Buffer & tf_ )
           : FootprintSubscriber(  node,   topic_name,   tf_ )
           {
           auto footprint = std::make_shared<geometry_msgs::msg::PolygonStamped>(   );
           footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup
           footprint->header.stamp = node->get_clock(   )->now(   );
           geometry_msgs::msg::Point32 point;
           point.x = -0.2f;
           point.y = -0.2f;
           footprint->polygon.points.push_back(  point );
           point.y = 0.2f;
           footprint->polygon.points.push_back(  point );
           point.x = 0.2f;
           point.y = 0.0f;
           footprint->polygon.points.push_back(  point );
          
           setFootprint(  footprint );
           }
          
     183   void setFootprint(  geometry_msgs::msg::PolygonStamped::SharedPtr msg )
           {
           footprint_ = msg;
           footprint_received_ = true;
           }
          };
          
     190  class DummySmootherServer : public nav2_smoother::SmootherServer
          {
          public:
     193   DummySmootherServer(   )
           {
           // Override defaults
           default_ids_.clear(   );
           default_ids_.resize(  1,   "SmoothPath" );
           set_parameter(  rclcpp::Parameter(  "smoother_plugins",   default_ids_ ) );
           default_types_.clear(   );
           default_types_.resize(  1,   "DummySmoother" );
           }
          
           nav2_util::CallbackReturn
     204   on_configure(  const rclcpp_lifecycle::State & state )
           {
           auto result = SmootherServer::on_configure(  state );
           if (  result != nav2_util::CallbackReturn::SUCCESS ) {
           return result;
           }
          
           // Create dummy subscribers and collision checker
           auto node = shared_from_this(   );
           costmap_sub_ =
           std::make_shared<DummyCostmapSubscriber>(  
           node,   "costmap_topic" );
           footprint_sub_ =
           std::make_shared<DummyFootprintSubscriber>(  
           node,   "footprint_topic",   *tf_ );
           collision_checker_ =
           std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(  
           *costmap_sub_,   *footprint_sub_,  
           node->get_name(   ) );
          
           return result;
           }
          };
          
          // Define a test class to hold the context for the tests
          
     230  class SmootherConfigTest : public ::testing::Test
          {
          };
          
     234  class SmootherTest : public ::testing::Test
          {
          protected:
     237   SmootherTest(   ) {SetUp(   );}
     238   ~SmootherTest(   ) {}
          
     240   void SetUp(   )
           {
           node_lifecycle_ =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  
           "LifecycleSmootherTestNode",   rclcpp::NodeOptions(   ) );
          
           smoother_server_ = std::make_shared<DummySmootherServer>(   );
           smoother_server_->set_parameter(  
           rclcpp::Parameter(  
           "smoother_plugins",  
           rclcpp::ParameterValue(  std::vector<std::string>(  1,   "DummySmoothPath" ) ) ) );
           smoother_server_->declare_parameter(  
           "DummySmoothPath.plugin",  
           rclcpp::ParameterValue(  std::string(  "DummySmoother" ) ) );
           smoother_server_->configure(   );
           smoother_server_->activate(   );
          
           client_ = rclcpp_action::create_client<SmoothAction>(  
           node_lifecycle_->get_node_base_interface(   ),  
           node_lifecycle_->get_node_graph_interface(   ),  
           node_lifecycle_->get_node_logging_interface(   ),  
           node_lifecycle_->get_node_waitables_interface(   ),   "smooth_path" );
           std::cout << "Setup complete." << std::endl;
           }
          
           void TearDown(   ) override
           {
           smoother_server_->deactivate(   );
           smoother_server_->cleanup(   );
           smoother_server_->shutdown(   );
           }
          
           bool sendGoal(  
           std::string smoother_id,   double x_start,   double y_start,   double x_goal,  
           double y_goal,   std::chrono::milliseconds max_time,   bool check_for_collisions )
           {
           if (  !client_->wait_for_action_server(  4s ) ) {
           std::cout << "Server not up" << std::endl;
           return false;
           }
          
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.orientation.w = 1.0;
          
           auto goal = SmoothAction::Goal(   );
           goal.smoother_id = smoother_id;
           pose.pose.position.x = x_start;
           pose.pose.position.y = y_start;
           goal.path.poses.push_back(  pose );
           pose.pose.position.x = x_goal;
           pose.pose.position.y = y_goal;
           goal.path.poses.push_back(  pose );
           goal.check_for_collisions = check_for_collisions;
           goal.max_smoothing_duration = rclcpp::Duration(  max_time );
          
           auto future_goal = client_->async_send_goal(  goal );
          
           if (  rclcpp::spin_until_future_complete(  node_lifecycle_,   future_goal ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           std::cout << "failed sending goal" << std::endl;
           // failed sending the goal
           return false;
           }
          
           goal_handle_ = future_goal.get(   );
          
           if (  !goal_handle_ ) {
           std::cout << "goal was rejected" << std::endl;
           // goal was rejected by the action server
           return false;
           }
          
           return true;
           }
          
           ClientGoalHandle::WrappedResult getResult(   )
           {
           std::cout << "Getting async result..." << std::endl;
           auto future_result = client_->async_get_result(  goal_handle_ );
           std::cout << "Waiting on future..." << std::endl;
           rclcpp::spin_until_future_complete(  node_lifecycle_,   future_result );
           std::cout << "future received!" << std::endl;
           return future_result.get(   );
           }
          
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
           std::shared_ptr<DummySmootherServer> smoother_server_;
           std::shared_ptr<rclcpp_action::Client<SmoothAction>> client_;
           std::shared_ptr<rclcpp_action::ClientGoalHandle<SmoothAction>> goal_handle_;
          };
          
          // Define the tests
          
          TEST_F(  SmootherTest,   testingSuccess )
          {
           ASSERT_TRUE(  sendGoal(  "DummySmoothPath",   0.0,   0.0,   1.0,   0.0,   500ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
           EXPECT_EQ(  result.result->path.poses.size(   ),   (  std::size_t )3 );
           EXPECT_TRUE(  result.result->was_completed );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingFailureOnInvalidSmootherId )
          {
           ASSERT_TRUE(  sendGoal(  "InvalidSmoother",   0.0,   0.0,   1.0,   0.0,   500ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::ABORTED );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingSuccessOnEmptyPlugin )
          {
           ASSERT_TRUE(  sendGoal(  "",   0.0,   0.0,   1.0,   0.0,   500ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingIncomplete )
          {
           ASSERT_TRUE(  sendGoal(  "DummySmoothPath",   0.0,   0.0,   1.0,   0.0,   50ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
           EXPECT_FALSE(  result.result->was_completed );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingFailureOnException )
          {
           ASSERT_TRUE(  sendGoal(  "DummySmoothPath",   0.0,   0.0,   0.0,   0.0,   500ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::ABORTED );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingFailureOnCollision )
          {
           ASSERT_TRUE(  sendGoal(  "DummySmoothPath",   -4.0,   0.0,   0.0,   0.0,   500ms,   true ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::ABORTED );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherTest,   testingCollisionCheckDisabled )
          {
           ASSERT_TRUE(  sendGoal(  "DummySmoothPath",   -4.0,   0.0,   0.0,   0.0,   500ms,   false ) );
           auto result = getResult(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
           SUCCEED(   );
          }
          
          TEST_F(  SmootherConfigTest,   testingConfigureSuccessWithValidSmootherPlugin )
          {
           auto smoother_server = std::make_shared<DummySmootherServer>(   );
           smoother_server->set_parameter(  
           rclcpp::Parameter(  
           "smoother_plugins",  
           rclcpp::ParameterValue(  std::vector<std::string>(  1,   "DummySmoothPath" ) ) ) );
           smoother_server->declare_parameter(  
           "DummySmoothPath.plugin",  
           rclcpp::ParameterValue(  std::string(  "DummySmoother" ) ) );
           auto state = smoother_server->configure(   );
           EXPECT_EQ(  state.id(   ),   2 ); // 1 on failure,   2 on success
           SUCCEED(   );
          }
          
          TEST_F(  SmootherConfigTest,   testingConfigureFailureWithInvalidSmootherPlugin )
          {
           auto smoother_server = std::make_shared<DummySmootherServer>(   );
           smoother_server->set_parameter(  
           rclcpp::Parameter(  
           "smoother_plugins",  
           rclcpp::ParameterValue(  std::vector<std::string>(  1,   "DummySmoothPath" ) ) ) );
           smoother_server->declare_parameter(  
           "DummySmoothPath.plugin",  
           rclcpp::ParameterValue(  std::string(  "InvalidSmootherPlugin" ) ) );
           auto state = smoother_server->configure(   );
           EXPECT_EQ(  state.id(   ),   1 ); // 1 on failure,   2 on success
           SUCCEED(   );
          }
          
          TEST_F(  SmootherConfigTest,   testingConfigureSuccessWithDefaultPlugin )
          {
           auto smoother_server = std::make_shared<DummySmootherServer>(   );
           auto state = smoother_server->configure(   );
           EXPECT_EQ(  state.id(   ),   2 ); // 1 on failure,   2 on success
           SUCCEED(   );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/behavior_tree/dummy_servers.hpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
          #define BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          #include <chrono>
          
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          using namespace std::chrono; // NOLINT
          using namespace std::placeholders; // NOLINT
          
          template<class ServiceT>
      32  class DummyService
          {
          public:
      35   explicit DummyService(  
      36   const rclcpp::Node::SharedPtr & node,  
      37   std::string service_name )
           : node_(  node ),  
           service_name_(  service_name ),  
           request_count_(  0 ),  
           disabled_(  false )
           {
           server_ = node->create_service<ServiceT>(  
           service_name,  
           std::bind(  &DummyService::handle_service,   this,   _1,   _2,   _3 ) );
           }
          
      48   void disable(   )
           {
           server_.reset(   );
           disabled_ = true;
           }
          
      54   void enable(   )
           {
           if (  disabled_ ) {
           server_ = node_->create_service<ServiceT>(  
           service_name_,  
           std::bind(  &DummyService::handle_service,   this,   _1,   _2,   _3 ) );
           disabled_ = false;
           }
           }
          
      64   void reset(   )
           {
           enable(   );
           request_count_ = 0;
           }
          
      70   int getRequestCount(   ) const
           {
           return request_count_;
           }
          
          protected:
      76   virtual void fillResponse(  
      77   const std::shared_ptr<typename ServiceT::Request>/*request*/,  
      78   const std::shared_ptr<typename ServiceT::Response>/*response*/ ) {}
          
      80   void handle_service(  
      81   const std::shared_ptr<rmw_request_id_t>/*request_header*/,  
      82   const std::shared_ptr<typename ServiceT::Request> request,  
      83   const std::shared_ptr<typename ServiceT::Response> response )
           {
           request_count_++;
           fillResponse(  request,   response );
           }
          
          private:
      90   rclcpp::Node::SharedPtr node_;
           typename rclcpp::Service<ServiceT>::SharedPtr server_;
           std::string service_name_;
           int request_count_;
           bool disabled_;
          };
          
          template<class ActionT>
      98  class DummyActionServer
          {
          public:
     101   explicit DummyActionServer(  
     102   const rclcpp::Node::SharedPtr & node,  
     103   std::string action_name )
           : action_name_(  action_name ),  
           goal_count_(  0 )
           {
           this->action_server_ = rclcpp_action::create_server<ActionT>(  
           node->get_node_base_interface(   ),  
           node->get_node_clock_interface(   ),  
           node->get_node_logging_interface(   ),  
           node->get_node_waitables_interface(   ),  
           action_name,  
           std::bind(  &DummyActionServer::handle_goal,   this,   _1,   _2 ),  
           std::bind(  &DummyActionServer::handle_cancel,   this,   _1 ),  
           std::bind(  &DummyActionServer::handle_accepted,   this,   _1 ) );
           }
          
     118   void setFailureRanges(  const std::vector<std::pair<int,   int>> & failureRanges )
           {
           failure_ranges_ = failureRanges;
           }
          
     123   void setRunningRanges(  const std::vector<std::pair<int,   int>> & runningRanges )
           {
           running_ranges_ = runningRanges;
           }
          
     128   void reset(   )
           {
           failure_ranges_.clear(   );
           running_ranges_.clear(   );
           goal_count_ = 0;
           }
          
     135   int getGoalCount(   ) const
           {
           return goal_count_;
           }
          
          protected:
     141   virtual std::shared_ptr<typename ActionT::Result> fillResult(   )
           {
           return std::make_shared<typename ActionT::Result>(   );
           }
          
     146   virtual rclcpp_action::GoalResponse handle_goal(  
     147   const rclcpp_action::GoalUUID &,  
     148   std::shared_ptr<const typename ActionT::Goal>/*goal*/ )
           {
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
           }
          
     153   virtual rclcpp_action::CancelResponse handle_cancel(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> )
           {
           return rclcpp_action::CancelResponse::ACCEPT;
           }
          
     159   void execute(  
           const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
           {
           goal_count_++;
           auto result = fillResult(   );
          
           // if current goal index exists in running range,   the thread sleeps for 1 second
           // to simulate a long running action
           for (  auto & index : running_ranges_ ) {
           if (  goal_count_ >= index.first && goal_count_ <= index.second ) {
           std::this_thread::sleep_for(  1s );
           break;
           }
           }
          
           // if current goal index exists in failure range,   the goal will be aborted
           for (  auto & index : failure_ranges_ ) {
           if (  goal_count_ >= index.first && goal_count_ <= index.second ) {
           goal_handle->abort(  result );
           return;
           }
           }
          
           // goal succeeds for all other indices
           goal_handle->succeed(  result );
           }
          
     186   void handle_accepted(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
           {
           using namespace std::placeholders; // NOLINT
           // this needs to return quickly to avoid blocking the executor,   so spin up a new thread
           std::thread{std::bind(  &DummyActionServer::execute,   this,   _1 ),   goal_handle}.detach(   );
           }
          
          protected:
           typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
           std::string action_name_;
          
           // contains pairs of indices which define a range for which the
           // requested action goal will return running for 1s or be aborted
           // for all other indices,   the action server will return success
           std::vector<std::pair<int,   int>> failure_ranges_;
           std::vector<std::pair<int,   int>> running_ranges_;
          
           int goal_count_;
          };
          
          #endif // BEHAVIOR_TREE__DUMMY_SERVERS_HPP_

navigation2/nav2_system_tests/src/behavior_tree/server_handler.cpp

       1  // Copyright (  c ) 2020 Vinny Ruia
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include <thread>
          
          #include "server_handler.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          using namespace std::chrono; // NOLINT
          
      25  ServerHandler::ServerHandler(   )
          : is_active_(  false )
          {
           node_ = rclcpp::Node::make_shared(  "behavior_tree_tester" );
          
           clear_local_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(  
           node_,   "local_costmap/clear_entirely_local_costmap" );
           clear_global_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(  
           node_,   "global_costmap/clear_entirely_global_costmap" );
           compute_path_to_pose_server = std::make_unique<ComputePathToPoseActionServer>(  node_ );
           follow_path_server = std::make_unique<DummyActionServer<nav2_msgs::action::FollowPath>>(  
           node_,   "follow_path" );
           spin_server = std::make_unique<DummyActionServer<nav2_msgs::action::Spin>>(  
           node_,   "spin" );
           wait_server = std::make_unique<DummyActionServer<nav2_msgs::action::Wait>>(  
           node_,   "wait" );
           backup_server = std::make_unique<DummyActionServer<nav2_msgs::action::BackUp>>(  
           node_,   "backup" );
           drive_on_heading_server = std::make_unique<DummyActionServer<nav2_msgs::action::DriveOnHeading>>(  
           node_,   "drive_on_heading" );
           ntp_server = std::make_unique<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>>(  
           node_,   "compute_path_through_poses" );
          }
          
      49  ServerHandler::~ServerHandler(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
      56  void ServerHandler::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already activated" );
           }
          
           is_active_ = true;
           server_thread_ =
           std::make_shared<std::thread>(  std::bind(  &ServerHandler::spinThread,   this ) );
          
           std::cout << "Server handler is active!" << std::endl;
          }
          
      69  void ServerHandler::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
          
           is_active_ = false;
           server_thread_->join(   );
          
           std::cout << "Server handler has been deactivated!" << std::endl;
          }
          
      81  void ServerHandler::reset(   ) const
          {
           clear_global_costmap_server->reset(   );
           clear_local_costmap_server->reset(   );
           compute_path_to_pose_server->reset(   );
           follow_path_server->reset(   );
           spin_server->reset(   );
           wait_server->reset(   );
           backup_server->reset(   );
           drive_on_heading_server->reset(   );
          }
          
      93  void ServerHandler::spinThread(   )
          {
           rclcpp::spin(  node_ );
          }

navigation2/nav2_system_tests/src/behavior_tree/server_handler.hpp

          // Copyright (  c ) 2020 Vinny Ruia
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIOR_TREE__SERVER_HANDLER_HPP_
          #define BEHAVIOR_TREE__SERVER_HANDLER_HPP_
          
          #include <memory>
          #include <string>
          #include <thread>
          #include <utility>
          #include <vector>
          
          #include "nav2_msgs/srv/clear_entire_costmap.hpp"
          #include "nav2_msgs/action/compute_path_to_pose.hpp"
          #include "nav2_msgs/action/follow_path.hpp"
          #include "nav2_msgs/action/spin.hpp"
          #include "nav2_msgs/action/back_up.hpp"
          #include "nav2_msgs/action/wait.hpp"
          #include "nav2_msgs/action/drive_on_heading.hpp"
          #include "nav2_msgs/action/compute_path_through_poses.hpp"
          
          #include "geometry_msgs/msg/point_stamped.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "dummy_servers.hpp"
          
      41  class ComputePathToPoseActionServer
      42   : public DummyActionServer<nav2_msgs::action::ComputePathToPose>
          {
          public:
      45   explicit ComputePathToPoseActionServer(  const rclcpp::Node::SharedPtr & node )
           : DummyActionServer(  node,   "compute_path_to_pose" )
           {
           result_ = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>(   );
           geometry_msgs::msg::PoseStamped pose;
           pose.header = result_->path.header;
           pose.pose.position.x = 0.0;
           pose.pose.position.y = 0.0;
           pose.pose.position.z = 0.0;
           pose.pose.orientation.x = 0.0;
           pose.pose.orientation.y = 0.0;
           pose.pose.orientation.z = 0.0;
           pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 6; ++i ) {
           result_->path.poses.push_back(  pose );
           }
           }
          
           std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> fillResult(   ) override
           {
           return result_;
           }
          
          private:
           std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> result_;
          };
          
          class ServerHandler
          {
          public:
           ServerHandler(   );
           ~ServerHandler(   );
          
           void activate(   );
          
           void deactivate(   );
          
           bool isActive(   ) const
           {
           return is_active_;
           }
          
           void reset(   ) const;
          
          public:
           std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_local_costmap_server;
           std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_global_costmap_server;
           std::unique_ptr<ComputePathToPoseActionServer> compute_path_to_pose_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::FollowPath>> follow_path_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::Spin>> spin_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::Wait>> wait_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::BackUp>> backup_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::DriveOnHeading>> drive_on_heading_server;
           std::unique_ptr<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>> ntp_server;
          
          private:
           void spinThread(   );
          
           bool is_active_;
           rclcpp::Node::SharedPtr node_;
           std::shared_ptr<std::thread> server_thread_;
          };
          
          #endif // BEHAVIOR_TREE__SERVER_HANDLER_HPP_

navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <vector>
          #include <string>
          #include <fstream>
          #include <memory>
          #include <utility>
          #include <boost/filesystem.hpp>
          
          #include "gtest/gtest.h"
          
          #include "behaviortree_cpp_v3/behavior_tree.h"
          #include "behaviortree_cpp_v3/bt_factory.h"
          #include "behaviortree_cpp_v3/utils/shared_library.h"
          
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/create_timer_ros.h"
          
          #include "rclcpp/rclcpp.hpp"
          #include "ament_index_cpp/get_package_share_directory.hpp"
          
          #include "server_handler.hpp"
          
          using namespace std::chrono_literals;
          namespace fs = boost::filesystem;
          
      40  class BehaviorTreeHandler
          {
          public:
      43   BehaviorTreeHandler(   )
           {
           node_ = rclcpp::Node::make_shared(  "behavior_tree_handler" );
          
           tf_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(  
           node_->get_node_base_interface(   ),   node_->get_node_timers_interface(   ) );
           tf_->setCreateTimerInterface(  timer_interface );
           tf_->setUsingDedicatedThread(  true );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_,   node_,   false );
          
           const std::vector<std::string> plugin_libs = {
           "nav2_compute_path_to_pose_action_bt_node",  
           "nav2_compute_path_through_poses_action_bt_node",  
           "nav2_smooth_path_action_bt_node",  
           "nav2_follow_path_action_bt_node",  
           "nav2_spin_action_bt_node",  
           "nav2_wait_action_bt_node",  
           "nav2_back_up_action_bt_node",  
           "nav2_drive_on_heading_bt_node",  
           "nav2_clear_costmap_service_bt_node",  
           "nav2_is_stuck_condition_bt_node",  
           "nav2_goal_reached_condition_bt_node",  
           "nav2_initial_pose_received_condition_bt_node",  
           "nav2_goal_updated_condition_bt_node",  
           "nav2_globally_updated_goal_condition_bt_node",  
           "nav2_is_path_valid_condition_bt_node",  
           "nav2_reinitialize_global_localization_service_bt_node",  
           "nav2_rate_controller_bt_node",  
           "nav2_distance_controller_bt_node",  
           "nav2_speed_controller_bt_node",  
           "nav2_truncate_path_action_bt_node",  
           "nav2_truncate_path_local_action_bt_node",  
           "nav2_goal_updater_node_bt_node",  
           "nav2_recovery_node_bt_node",  
           "nav2_pipeline_sequence_bt_node",  
           "nav2_round_robin_node_bt_node",  
           "nav2_transform_available_condition_bt_node",  
           "nav2_time_expired_condition_bt_node",  
           "nav2_path_expiring_timer_condition",  
           "nav2_distance_traveled_condition_bt_node",  
           "nav2_single_trigger_bt_node",  
           "nav2_is_battery_low_condition_bt_node",  
           "nav2_navigate_through_poses_action_bt_node",  
           "nav2_navigate_to_pose_action_bt_node",  
           "nav2_remove_passed_goals_action_bt_node",  
           "nav2_planner_selector_bt_node",  
           "nav2_controller_selector_bt_node",  
           "nav2_goal_checker_selector_bt_node",  
           "nav2_controller_cancel_bt_node",  
           "nav2_path_longer_on_approach_bt_node",  
           "nav2_wait_cancel_bt_node",  
           "nav2_spin_cancel_bt_node",  
           "nav2_back_up_cancel_bt_node",  
           "nav2_drive_on_heading_cancel_bt_node"
           };
           for (  const auto & p : plugin_libs ) {
           factory_.registerFromPlugin(  BT::SharedLibrary::getOSName(  p ) );
           }
           }
          
     104   bool loadBehaviorTree(  const std::string & filename )
           {
           // Read the input BT XML from the specified file into a string
           std::ifstream xml_file(  filename );
          
           if (  !xml_file.good(   ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Couldn't open input XML file: %s",   filename.c_str(   ) );
           return false;
           }
          
           auto xml_string = std::string(  
           std::istreambuf_iterator<char>(  xml_file ),  
           std::istreambuf_iterator<char>(   ) );
          
           // Create the blackboard that will be shared by all of the nodes in the tree
           blackboard = BT::Blackboard::create(   );
          
           // Put items on the blackboard
           blackboard->set<rclcpp::Node::SharedPtr>(  "node",   node_ ); // NOLINT
           blackboard->set<std::chrono::milliseconds>(  
           "server_timeout",   std::chrono::milliseconds(  20 ) ); // NOLINT
           blackboard->set<std::chrono::milliseconds>(  
           "bt_loop_duration",   std::chrono::milliseconds(  10 ) ); // NOLINT
           blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(  "tf_buffer",   tf_ ); // NOLINT
           blackboard->set<bool>(  "initial_pose_received",   false ); // NOLINT
           blackboard->set<int>(  "number_recoveries",   0 ); // NOLINT
          
           // set dummy goal on blackboard
           geometry_msgs::msg::PoseStamped goal;
           goal.header.stamp = node_->now(   );
           goal.header.frame_id = "map";
           goal.pose.position.x = 0.0;
           goal.pose.position.y = 0.0;
           goal.pose.position.z = 0.0;
           goal.pose.orientation.x = 0.0;
           goal.pose.orientation.y = 0.0;
           goal.pose.orientation.z = 0.0;
           goal.pose.orientation.w = 1.0;
          
           blackboard->set<geometry_msgs::msg::PoseStamped>(  "goal",   goal ); // NOLINT
          
           // Create the Behavior Tree from the XML input
           try {
           tree = factory_.createTreeFromText(  xml_string,   blackboard );
           } catch (  BT::RuntimeError & exp ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "%s: %s",   filename.c_str(   ),   exp.what(   ) );
           return false;
           }
          
           return true;
           }
          
          public:
     157   BT::Blackboard::Ptr blackboard;
     158   BT::Tree tree;
          
          private:
     161   rclcpp::Node::SharedPtr node_;
          
     163   std::shared_ptr<tf2_ros::Buffer> tf_;
     164   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          
     166   BT::BehaviorTreeFactory factory_;
          };
          
     169  class BehaviorTreeTestFixture : public ::testing::Test
          {
          public:
     172   static void SetUpTestCase(   )
           {
           // initialize ROS
           rclcpp::init(  0,   nullptr );
          
           server_handler = std::make_shared<ServerHandler>(   );
           if (  !server_handler->isActive(   ) ) {
           server_handler->activate(   );
           }
           }
          
     183   static void TearDownTestCase(   )
           {
           // shutdown ROS
           rclcpp::shutdown(   );
          
           server_handler.reset(   );
           bt_handler.reset(   );
           }
          
           void SetUp(   ) override
           {
           server_handler->reset(   );
           bt_handler = std::make_shared<BehaviorTreeHandler>(   );
           }
          
           void TearDown(   ) override
           {
           bt_handler.reset(   );
           }
          
          protected:
           static std::shared_ptr<ServerHandler> server_handler;
           static std::shared_ptr<BehaviorTreeHandler> bt_handler;
          };
          
          std::shared_ptr<ServerHandler> BehaviorTreeTestFixture::server_handler = nullptr;
          std::shared_ptr<BehaviorTreeHandler> BehaviorTreeTestFixture::bt_handler = nullptr;
          
          TEST_F(  BehaviorTreeTestFixture,   TestBTXMLFiles )
          {
           fs::path root = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           root /= "behavior_trees/";
          
           if (  boost::filesystem::exists(  root ) && boost::filesystem::is_directory(  root ) ) {
           for (  auto const & entry : boost::filesystem::recursive_directory_iterator(  root ) ) {
           if (  boost::filesystem::is_regular_file(  entry ) && entry.path(   ).extension(   ) == ".xml" ) {
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  entry.path(   ).string(   ) ),   true );
           }
           }
           }
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose and FollowPath return SUCCESS
           * The behavior tree should execute correctly and return SUCCESS
           */
          TEST_F(  BehaviorTreeTestFixture,   TestAllSuccess )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be success since all action servers returned success
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          
           // Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   1 );
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   1 );
          
           // Goal count should be 0 since no goal is sent to all other servers
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   0 );
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   0 );
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose returns FAILURE and ClearGlobalCostmap-Context returns FAILURE
           * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE and RoundRobin is triggered
           * RoundRobin triggers ClearingActions Sequence which returns FAILURE
           * RoundRobin triggers Spin,   Wait,   and BackUp which return FAILURE
           * RoundRobin returns FAILURE hence RecoveryCallbackk returns FAILURE
           * Finally NavigateRecovery returns FAILURE
           * The behavior tree should also return FAILURE
           */
          TEST_F(  BehaviorTreeTestFixture,   TestAllFailure )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           // Set all action server to fail the first 100 times
           std::vector<std::pair<int,   int>> failureRange;
           failureRange.emplace_back(  std::pair<int,   int>(  0,   100 ) );
           server_handler->compute_path_to_pose_server->setFailureRanges(  failureRange );
           server_handler->follow_path_server->setFailureRanges(  failureRange );
           server_handler->spin_server->setFailureRanges(  failureRange );
           server_handler->wait_server->setFailureRanges(  failureRange );
           server_handler->backup_server->setFailureRanges(  failureRange );
          
           // Disable services
           server_handler->clear_global_costmap_server->disable(   );
           server_handler->clear_local_costmap_server->disable(   );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be failure
           EXPECT_EQ(  result,   BT::NodeStatus::FAILURE );
          
           // Goal count should be 1 since only one goal is sent to ComputePathToPose
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   1 );
          
           // Goal count should be 0 since no goal is sent to FollowPath action server
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   0 );
          
           // All recovery action servers were sent 1 goal
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   1 );
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   1 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   1 );
          
           // Service count is 0 since the server was disabled
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   0 );
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   0 );
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
           * FollowPath returns FAILURE on the first try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS when retried
           * The behavior tree should return SUCCESS
           */
          TEST_F(  BehaviorTreeTestFixture,   TestNavigateSubtreeRecoveries )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           // Set ComputePathToPose and FollowPath action servers to fail for the first action
           std::vector<std::pair<int,   int>> failureRange;
           failureRange.emplace_back(  std::pair<int,   int>(  0,   1 ) );
           server_handler->compute_path_to_pose_server->setFailureRanges(  failureRange );
           server_handler->follow_path_server->setFailureRanges(  failureRange );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be success
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          
           // Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   2 );
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   2 );
          
           // Navigate subtree recovery services are called once each
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   1 );
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   1 );
          
           // Goal count should be 0 since no goal is sent to all other servers
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   0 );
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
           * FollowPath returns FAILURE on the first try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
           * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
           * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
           * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
           * FollowPath returns FAILURE on the third try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS on the fourth try
           * The behavior tree should return SUCCESS
           */
          TEST_F(  BehaviorTreeTestFixture,   TestNavigateRecoverySimple )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           // Set ComputePathToPose action server to fail for the first action
           std::vector<std::pair<int,   int>> plannerFailureRange;
           plannerFailureRange.emplace_back(  std::pair<int,   int>(  0,   1 ) );
           server_handler->compute_path_to_pose_server->setFailureRanges(  plannerFailureRange );
          
           // Set FollowPath action server to fail for the first 3 actions
           std::vector<std::pair<int,   int>> controllerFailureRange;
           controllerFailureRange.emplace_back(  std::pair<int,   int>(  0,   3 ) );
           server_handler->follow_path_server->setFailureRanges(  controllerFailureRange );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be success
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          
           // FollowPath is called 4 times
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   4 );
          
           // ComputePathToPose is called 3 times
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   3 );
          
           // Local costmap is cleared 3 times
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   3 );
          
           // Global costmap is cleared 2 times
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   2 );
          
           // Goal count should be 0 since only no goal is sent to all other servers
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   0 );
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE,   RoundRobin triggers ClearingActions Sequence which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (  retry #1 )
           * FollowPath returns FAILURE on the first try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
           * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
           * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
           * RoundRobin triggers Spin which returns FAILURE
           * RoundRobin triggers Wait which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (  retry #2 )
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE and RoundRobin triggers BackUp which returns FAILURE
           * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (  retry #3 )
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE and RoundRobin triggers Spin which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (  retry #4 )
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE and RoundRobin triggers Wait which returns FAILURE
           * RoundRobin triggers BackUp which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (  retry #5 )
           * FollowPath returns FAILURE on the first try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
           * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
           * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
           * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           *
           * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (  retry #6 )
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE and NavigateRecovery finally also returns FAILURE
           *
           * The behavior tree should return FAILURE
           */
          TEST_F(  BehaviorTreeTestFixture,   TestNavigateRecoveryComplex )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           // Set ComputePathToPose action server to fail for the first 2 actions
           std::vector<std::pair<int,   int>> plannerFailureRange;
           plannerFailureRange.emplace_back(  std::pair<int,   int>(  0,   2 ) );
           plannerFailureRange.emplace_back(  std::pair<int,   int>(  4,   9 ) );
           plannerFailureRange.emplace_back(  std::pair<int,   int>(  11,   12 ) );
           server_handler->compute_path_to_pose_server->setFailureRanges(  plannerFailureRange );
          
           // Set FollowPath action server to fail for the first 2 actions
           std::vector<std::pair<int,   int>> controllerFailureRange;
           controllerFailureRange.emplace_back(  std::pair<int,   int>(  0,   4 ) );
           server_handler->follow_path_server->setFailureRanges(  controllerFailureRange );
          
           // Set Spin action server to fail for the first action
           std::vector<std::pair<int,   int>> spinFailureRange;
           spinFailureRange.emplace_back(  std::pair<int,   int>(  0,   1 ) );
           server_handler->spin_server->setFailureRanges(  spinFailureRange );
          
           // Set Wait action server to fail for the first action
           std::vector<std::pair<int,   int>> waitFailureRange;
           waitFailureRange.emplace_back(  std::pair<int,   int>(  2,   2 ) );
           server_handler->wait_server->setFailureRanges(  waitFailureRange );
          
           // Set BackUp action server to fail for the first action
           std::vector<std::pair<int,   int>> backupFailureRange;
           backupFailureRange.emplace_back(  std::pair<int,   int>(  0,   1 ) );
           server_handler->backup_server->setFailureRanges(  backupFailureRange );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be success
           EXPECT_EQ(  result,   BT::NodeStatus::FAILURE );
          
           // ComputePathToPose is called 12 times
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   12 );
          
           // FollowPath is called 4 times
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   4 );
          
           // Local costmap is cleared 5 times
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   5 );
          
           // Global costmap is cleared 8 times
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   8 );
          
           // All recovery action servers receive 2 goals
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   2 );
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   2 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   2 );
          }
          
          /**
           * Test scenario:
           *
           * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
           * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
           * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
           * GoalUpdated returns FAILURE,   RoundRobin triggers ClearingActions Sequence which returns SUCCESS
           * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
           * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
           * FollowPath returns FAILURE on the first try triggering the controller recovery
           * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
           * FollowPath returns FAILURE and PipelineSequence returns FAILURE
           * NavigateRecovery triggers RecoveryFallback which triggers GoalUpdated
           * GoalUpdated returns FAILURE and RecoveryFallback triggers RoundRobin
           * RoundRobin triggers Spin which returns RUNNING
           *
           * At this point a new goal is updated on the blackboard
           *
           * RecoveryFallback triggers GoalUpdated which returns SUCCESS this time
           * Since GoalUpdated returned SUCCESS,   RoundRobin and hence Spin is halted
           * RecoveryFallback also returns SUCCESS and PipelineSequence is retried
           * PipelineSequence triggers ComputePathToPose which returns SUCCESS
           * FollowPath returns SUCCESS and NavigateRecovery finally also returns SUCCESS
           *
           * The behavior tree should return SUCCESS
           */
          TEST_F(  BehaviorTreeTestFixture,   TestRecoverySubtreeGoalUpdated )
          {
           // Load behavior tree from file
           fs::path bt_file = ament_index_cpp::get_package_share_directory(  "nav2_bt_navigator" );
           bt_file /= "behavior_trees/";
           bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
           EXPECT_EQ(  bt_handler->loadBehaviorTree(  bt_file.string(   ) ),   true );
          
           // Set ComputePathToPose action server to fail for the first 2 actions
           std::vector<std::pair<int,   int>> plannerFailureRange;
           plannerFailureRange.emplace_back(  std::pair<int,   int>(  0,   2 ) );
           server_handler->compute_path_to_pose_server->setFailureRanges(  plannerFailureRange );
          
           // Set FollowPath action server to fail for the first 2 actions
           std::vector<std::pair<int,   int>> controllerFailureRange;
           controllerFailureRange.emplace_back(  std::pair<int,   int>(  0,   2 ) );
           server_handler->follow_path_server->setFailureRanges(  controllerFailureRange );
          
           // Set Spin action server to return running for the first action
           std::vector<std::pair<int,   int>> spinRunningRange;
           spinRunningRange.emplace_back(  std::pair<int,   int>(  1,   1 ) );
           server_handler->spin_server->setRunningRanges(  spinRunningRange );
          
           BT::NodeStatus result = BT::NodeStatus::RUNNING;
          
           while (  result == BT::NodeStatus::RUNNING ) {
           result = bt_handler->tree.tickRoot(   );
          
           // Update goal on blackboard after Spin has been triggered once
           // to simulate a goal update during a recovery action
           if (  server_handler->spin_server->getGoalCount(   ) > 0 ) {
           geometry_msgs::msg::PoseStamped goal;
           goal.pose.position.x = 1.0;
           goal.pose.position.y = 1.0;
           goal.pose.position.z = 1.0;
           goal.pose.orientation.x = 0.0;
           goal.pose.orientation.y = 0.0;
           goal.pose.orientation.z = 0.0;
           goal.pose.orientation.w = 1.0;
           bt_handler->blackboard->set<geometry_msgs::msg::PoseStamped>(  "goal",   goal ); // NOLINT
           }
          
           std::this_thread::sleep_for(  10ms );
           }
          
           // The final result should be success
           EXPECT_EQ(  result,   BT::NodeStatus::SUCCESS );
          
           // ComputePathToPose is called 4 times
           EXPECT_EQ(  server_handler->compute_path_to_pose_server->getGoalCount(   ),   4 );
          
           // FollowPath is called 3 times
           EXPECT_EQ(  server_handler->follow_path_server->getGoalCount(   ),   3 );
          
           // Local costmap is cleared 2 times
           EXPECT_EQ(  server_handler->clear_local_costmap_server->getRequestCount(   ),   2 );
          
           // Global costmap is cleared 2 times
           EXPECT_EQ(  server_handler->clear_global_costmap_server->getRequestCount(   ),   2 );
          
           // Spin server receives 1 action
           EXPECT_EQ(  server_handler->spin_server->getGoalCount(   ),   1 );
          
           // All recovery action servers receive 0 goals
           EXPECT_EQ(  server_handler->wait_server->getGoalCount(   ),   0 );
           EXPECT_EQ(  server_handler->backup_server->getGoalCount(   ),   0 );
          }
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
           bool all_successful = RUN_ALL_TESTS(   );
           return all_successful;
          }

navigation2/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <random>
          #include <tuple>
          #include <memory>
          #include <iostream>
          #include <chrono>
          #include <sstream>
          #include <iomanip>
          
          #include "backup_behavior_tester.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          
          namespace nav2_system_tests
          {
          
      34  BackupBehaviorTester::BackupBehaviorTester(   )
          : is_active_(  false ),  
           initial_pose_received_(  false )
          {
           node_ = rclcpp::Node::make_shared(  "backup_behavior_test" );
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
          
           client_ptr_ = rclcpp_action::create_client<BackUp>(  
           node_->get_node_base_interface(   ),  
           node_->get_node_graph_interface(   ),  
           node_->get_node_logging_interface(   ),  
           node_->get_node_waitables_interface(   ),  
           "backup" );
          
           publisher_ =
           node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  "initialpose",   10 );
          
           subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &BackupBehaviorTester::amclPoseCallback,   this,   std::placeholders::_1 ) );
          
           stamp_ = node_->now(   );
          }
          
      60  BackupBehaviorTester::~BackupBehaviorTester(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
      67  void BackupBehaviorTester::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           return;
           }
          
           while (  !initial_pose_received_ ) {
           RCLCPP_WARN(  node_->get_logger(   ),   "Initial pose not received" );
           sendInitialPose(   );
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node_ );
           }
          
           // Wait for lifecycle_manager_navigation to activate behavior_server
           std::this_thread::sleep_for(  10s );
          
           if (  !client_ptr_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action client not initialized" );
           is_active_ = false;
           return;
           }
          
           if (  !client_ptr_->wait_for_action_server(  10s ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action server not available after waiting" );
           is_active_ = false;
           return;
           }
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Backup action server is ready" );
           is_active_ = true;
          }
          
     100  void BackupBehaviorTester::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
           is_active_ = false;
          }
          
     108  bool BackupBehaviorTester::defaultBackupBehaviorTest(  
     109   const BackUp::Goal goal_msg,  
           const double tolerance )
          {
           if (  !is_active_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Not activated" );
           return false;
           }
          
           // Sleep to let behavior server be ready for serving in multiple runs
           std::this_thread::sleep_for(  5s );
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Sending goal" );
          
           geometry_msgs::msg::PoseStamped initial_pose;
           if (  !nav2_util::getCurrentPose(  initial_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
           RCLCPP_INFO(  node_->get_logger(   ),   "Found current robot pose" );
          
           auto goal_handle_future = client_ptr_->async_send_goal(  goal_msg );
          
           if (  rclcpp::spin_until_future_complete(  node_,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "send goal call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<BackUp>::SharedPtr goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Goal was rejected by server" );
           return false;
           }
          
           // Wait for the server to be done with the goal
           auto result_future = client_ptr_->async_get_result(  goal_handle );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for result" );
           if (  rclcpp::spin_until_future_complete(  node_,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "get result call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<BackUp>::WrappedResult wrapped_result = result_future.get(   );
          
           switch (  wrapped_result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED: break;
           case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was aborted" );
           return false;
           case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was canceled" );
           return false;
           default: RCLCPP_ERROR(  node_->get_logger(   ),   "Unknown result code" );
           return false;
           }
          
           RCLCPP_INFO(  node_->get_logger(   ),   "result received" );
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  current_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
          
           double dist = nav2_util::geometry_utils::euclidean_distance(  initial_pose,   current_pose );
          
           if (  fabs(  dist ) > fabs(  goal_msg.target.x ) + tolerance ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Distance from goal is %lf (  tolerance %lf )",  
           fabs(  dist - goal_msg.target.x ),   tolerance );
           return false;
           }
          
           return true;
          }
          
     192  void BackupBehaviorTester::sendInitialPose(   )
          {
           geometry_msgs::msg::PoseWithCovarianceStamped pose;
           pose.header.frame_id = "map";
           pose.header.stamp = stamp_;
           pose.pose.pose.position.x = -2.0;
           pose.pose.pose.position.y = -0.5;
           pose.pose.pose.position.z = 0.0;
           pose.pose.pose.orientation.x = 0.0;
           pose.pose.pose.orientation.y = 0.0;
           pose.pose.pose.orientation.z = 0.0;
           pose.pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 35; i++ ) {
           pose.pose.covariance[i] = 0.0;
           }
           pose.pose.covariance[0] = 0.08;
           pose.pose.covariance[7] = 0.08;
           pose.pose.covariance[35] = 0.05;
          
           publisher_->publish(  pose );
           RCLCPP_INFO(  node_->get_logger(   ),   "Sent initial pose" );
          }
          
     215  void BackupBehaviorTester::amclPoseCallback(  
     216   const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
          {
           initial_pose_received_ = true;
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp

          // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
          #define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <thread>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "angles/angles.h"
          #include "nav2_msgs/action/back_up.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
          
          #include "tf2/utils.h"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          
          namespace nav2_system_tests
          {
          
      41  class BackupBehaviorTester
          {
          public:
           using BackUp = nav2_msgs::action::BackUp;
           using GoalHandleBackup = rclcpp_action::ClientGoalHandle<BackUp>;
          
           BackupBehaviorTester(   );
           ~BackupBehaviorTester(   );
          
           // Runs a single test with given target yaw
           bool defaultBackupBehaviorTest(  
           const BackUp::Goal goal_msg,  
           const double tolerance );
          
           void activate(   );
          
           void deactivate(   );
          
           bool isActive(   ) const
           {
           return is_active_;
           }
          
          private:
           void sendInitialPose(   );
          
      67   void amclPoseCallback(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
          
           bool is_active_;
           bool initial_pose_received_;
           rclcpp::Time stamp_;
          
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          
           rclcpp::Node::SharedPtr node_;
          
           // Publisher to publish initial pose
           rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
          
           // Subscriber for amcl pose
           rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
          
           // Action client to call Backup action
           rclcpp_action::Client<BackUp>::SharedPtr client_ptr_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <cmath>
          #include <tuple>
          #include <string>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "backup_behavior_tester.hpp"
          #include "nav2_msgs/action/back_up.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::BackupBehaviorTester;
          
          struct TestParameters
          {
           float x;
           float y;
           float speed;
           float tolerance;
          };
          
          
      41  std::string testNameGenerator(  const testing::TestParamInfo<TestParameters> & )
          {
           static int test_index = 0;
           std::string name = "BackUpTest" + std::to_string(  test_index );
           ++test_index;
           return name;
          }
          
      49  class BackupBehaviorTestFixture
      50   : public ::testing::TestWithParam<TestParameters>
          {
          public:
      53   static void SetUpTestCase(   )
           {
           backup_behavior_tester = new BackupBehaviorTester(   );
           if (  !backup_behavior_tester->isActive(   ) ) {
           backup_behavior_tester->activate(   );
           }
           }
          
      61   static void TearDownTestCase(   )
           {
           delete backup_behavior_tester;
           backup_behavior_tester = nullptr;
           }
          
          protected:
      68   static BackupBehaviorTester * backup_behavior_tester;
          };
          
          BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr;
          
      73  TEST_P(  BackupBehaviorTestFixture,   testBackupBehavior )
          {
           auto test_params = GetParam(   );
           auto goal = nav2_msgs::action::BackUp::Goal(   );
           goal.target.x = test_params.x;
           goal.target.y = test_params.y;
           goal.speed = test_params.speed;
           float tolerance = test_params.tolerance;
          
           if (  !backup_behavior_tester->isActive(   ) ) {
           backup_behavior_tester->activate(   );
           }
          
           bool success = false;
           success = backup_behavior_tester->defaultBackupBehaviorTest(  goal,   tolerance );
          
           float dist_to_obstacle = 2.0f;
          
           if (   (  (  dist_to_obstacle - std::fabs(  test_params.x ) ) < std::fabs(  goal.speed ) ) ||
           std::fabs(  goal.target.y ) > 0 )
           {
           EXPECT_FALSE(  success );
           } else {
           EXPECT_TRUE(  success );
           }
          }
          
          std::vector<TestParameters> test_params = {TestParameters{-0.05,   0.0,   -0.2,   0.01},  
           TestParameters{-0.05,   0.1,   -0.2,   0.01},  
           TestParameters{-2.0,   0.0,   -0.2,   0.1}};
          
     104  INSTANTIATE_TEST_SUITE_P(  
           BackupBehaviorTests,  
           BackupBehaviorTestFixture,  
           ::testing::Values(  
           test_params[0],  
           test_params[1],  
           test_params[2] ),  
           testNameGenerator
           );
          
          
     115  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <random>
          #include <tuple>
          #include <memory>
          #include <iostream>
          #include <chrono>
          #include <sstream>
          #include <iomanip>
          
          #include "drive_on_heading_behavior_tester.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          
          namespace nav2_system_tests
          {
          
      34  DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester(   )
          : is_active_(  false ),  
           initial_pose_received_(  false )
          {
           node_ = rclcpp::Node::make_shared(  "DriveOnHeading_behavior_test" );
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
          
           client_ptr_ = rclcpp_action::create_client<DriveOnHeading>(  
           node_->get_node_base_interface(   ),  
           node_->get_node_graph_interface(   ),  
           node_->get_node_logging_interface(   ),  
           node_->get_node_waitables_interface(   ),  
           "drive_on_heading" );
          
           publisher_ =
           node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  "initialpose",   10 );
          
           subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &DriveOnHeadingBehaviorTester::amclPoseCallback,   this,   std::placeholders::_1 ) );
          
           stamp_ = node_->now(   );
          }
          
      60  DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
      67  void DriveOnHeadingBehaviorTester::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           return;
           }
          
           while (  !initial_pose_received_ ) {
           RCLCPP_WARN(  node_->get_logger(   ),   "Initial pose not received" );
           sendInitialPose(   );
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node_ );
           }
          
           // Wait for lifecycle_manager_navigation to activate behavior_server
           std::this_thread::sleep_for(  10s );
          
           if (  !client_ptr_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action client not initialized" );
           is_active_ = false;
           return;
           }
          
           if (  !client_ptr_->wait_for_action_server(  10s ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action server not available after waiting" );
           is_active_ = false;
           return;
           }
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "DriveOnHeading action server is ready" );
           is_active_ = true;
          }
          
     100  void DriveOnHeadingBehaviorTester::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
           is_active_ = false;
          }
          
     108  bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest(  
     109   const DriveOnHeading::Goal goal_msg,  
           const double tolerance )
          {
           if (  !is_active_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Not activated" );
           return false;
           }
          
           // Sleep to let behavior server be ready for serving in multiple runs
           std::this_thread::sleep_for(  5s );
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Sending goal" );
          
           geometry_msgs::msg::PoseStamped initial_pose;
           if (  !nav2_util::getCurrentPose(  initial_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
           RCLCPP_INFO(  node_->get_logger(   ),   "Found current robot pose" );
          
           auto goal_handle_future = client_ptr_->async_send_goal(  goal_msg );
          
           if (  rclcpp::spin_until_future_complete(  node_,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "send goal call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<DriveOnHeading>::SharedPtr goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Goal was rejected by server" );
           return false;
           }
          
           // Wait for the server to be done with the goal
           auto result_future = client_ptr_->async_get_result(  goal_handle );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for result" );
           if (  rclcpp::spin_until_future_complete(  node_,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "get result call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<DriveOnHeading>::WrappedResult wrapped_result =
           result_future.get(   );
          
           switch (  wrapped_result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED: break;
           case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was aborted" );
           return false;
           case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was canceled" );
           return false;
           default: RCLCPP_ERROR(  node_->get_logger(   ),   "Unknown result code" );
           return false;
           }
          
           RCLCPP_INFO(  node_->get_logger(   ),   "result received" );
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  current_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
          
           double dist = nav2_util::geometry_utils::euclidean_distance(  initial_pose,   current_pose );
          
           if (  fabs(  dist ) > fabs(  goal_msg.target.x ) + tolerance ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Distance from goal is %lf (  tolerance %lf )",  
           fabs(  dist - goal_msg.target.x ),   tolerance );
           return false;
           }
          
           return true;
          }
          
     193  void DriveOnHeadingBehaviorTester::sendInitialPose(   )
          {
           geometry_msgs::msg::PoseWithCovarianceStamped pose;
           pose.header.frame_id = "map";
           pose.header.stamp = stamp_;
           pose.pose.pose.position.x = -2.0;
           pose.pose.pose.position.y = -0.5;
           pose.pose.pose.position.z = 0.0;
           pose.pose.pose.orientation.x = 0.0;
           pose.pose.pose.orientation.y = 0.0;
           pose.pose.pose.orientation.z = 0.0;
           pose.pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 35; i++ ) {
           pose.pose.covariance[i] = 0.0;
           }
           pose.pose.covariance[0] = 0.08;
           pose.pose.covariance[7] = 0.08;
           pose.pose.covariance[35] = 0.05;
          
           publisher_->publish(  pose );
           RCLCPP_INFO(  node_->get_logger(   ),   "Sent initial pose" );
          }
          
     216  void DriveOnHeadingBehaviorTester::amclPoseCallback(  
     217   const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
          {
           initial_pose_received_ = true;
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp

          // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
          #define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <thread>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "angles/angles.h"
          #include "nav2_msgs/action/drive_on_heading.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
          
          #include "tf2/utils.h"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          
          namespace nav2_system_tests
          {
          
      41  class DriveOnHeadingBehaviorTester
          {
          public:
           using DriveOnHeading = nav2_msgs::action::DriveOnHeading;
           using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle<DriveOnHeading>;
          
           DriveOnHeadingBehaviorTester(   );
           ~DriveOnHeadingBehaviorTester(   );
          
           // Runs a single test with given target yaw
           bool defaultDriveOnHeadingBehaviorTest(  
           const DriveOnHeading::Goal goal_msg,  
           double tolerance );
          
           void activate(   );
          
           void deactivate(   );
          
           bool isActive(   ) const
           {
           return is_active_;
           }
          
          private:
           void sendInitialPose(   );
          
      67   void amclPoseCallback(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
          
           bool is_active_;
           bool initial_pose_received_;
           rclcpp::Time stamp_;
          
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          
           rclcpp::Node::SharedPtr node_;
          
           // Publisher to publish initial pose
           rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
          
           // Subscriber for amcl pose
           rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
          
           // Action client to call DriveOnHeading action
           rclcpp_action::Client<DriveOnHeading>::SharedPtr client_ptr_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <cmath>
          #include <tuple>
          #include <string>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "drive_on_heading_behavior_tester.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::DriveOnHeadingBehaviorTester;
          
          struct TestParameters
          {
           float x;
           float y;
           float speed;
           float time_allowance;
           float tolerance;
          };
          
      39  std::string testNameGenerator(  const testing::TestParamInfo<TestParameters> & )
          {
           static int test_index = 0;
           std::string name = "DriveOnHeadingTest" + std::to_string(  test_index );
           ++test_index;
           return name;
          }
          
      47  class DriveOnHeadingBehaviorTestFixture
      48   : public ::testing::TestWithParam<TestParameters>
          {
          public:
      51   static void SetUpTestCase(   )
           {
           drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester(   );
           if (  !drive_on_heading_behavior_tester->isActive(   ) ) {
           drive_on_heading_behavior_tester->activate(   );
           }
           }
          
      59   static void TearDownTestCase(   )
           {
           delete drive_on_heading_behavior_tester;
           drive_on_heading_behavior_tester = nullptr;
           }
          
          protected:
      66   static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester;
          };
          
          DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester =
           nullptr;
          
      72  TEST_P(  DriveOnHeadingBehaviorTestFixture,   testBackupBehavior )
          {
           auto test_params = GetParam(   );
           auto goal = nav2_msgs::action::DriveOnHeading::Goal(   );
           goal.target.x = test_params.x;
           goal.target.y = test_params.y;
           goal.speed = test_params.speed;
           goal.time_allowance.sec = test_params.time_allowance;
           float tolerance = test_params.tolerance;
          
           if (  !drive_on_heading_behavior_tester->isActive(   ) ) {
           drive_on_heading_behavior_tester->activate(   );
           }
          
           bool success = false;
           success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest(  
           goal,  
           tolerance );
          
           float dist_to_obstacle = 2.0f;
           if (   (  (  dist_to_obstacle - std::fabs(  test_params.x ) ) < std::fabs(  goal.speed ) ) ||
           std::fabs(  goal.target.y ) > 0 ||
           goal.time_allowance.sec < 2.0 ||
           !(  (  goal.target.x > 0.0 ) == (  goal.speed > 0.0 ) ) )
           {
           EXPECT_FALSE(  success );
           } else {
           EXPECT_TRUE(  success );
           }
          }
          
          std::vector<TestParameters> test_params = {TestParameters{-0.05,   0.0,   -0.2,   10.0,   0.01},  
           TestParameters{-0.05,   0.1,   -0.2,   10.0,   0.01},  
           TestParameters{-2.0,   0.0,   -0.2,   10.0,   0.1},  
           TestParameters{-0.05,   0.0,   -0.01,   1.0,   0.01},  
           TestParameters{0.05,   0.0,   -0.2,   10.0,   0.01}};
          
     109  INSTANTIATE_TEST_SUITE_P(  
           DriveOnHeadingBehaviorTests,  
           DriveOnHeadingBehaviorTestFixture,  
           ::testing::Values(  
           test_params[0],  
           test_params[1],  
           test_params[2],  
           test_params[3],  
           test_params[4] ),  
           testNameGenerator );
          
     120  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <random>
          #include <tuple>
          #include <memory>
          #include <iostream>
          #include <chrono>
          #include <sstream>
          #include <iomanip>
          
          #include "spin_behavior_tester.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          
          namespace nav2_system_tests
          {
          
      33  SpinBehaviorTester::SpinBehaviorTester(   )
          : is_active_(  false ),  
           initial_pose_received_(  false )
          {
           node_ = rclcpp::Node::make_shared(  "spin_behavior_test" );
          
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
           tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(  node_ );
           if (  std::getenv(  "MAKE_FAKE_COSTMAP" ) != NULL ) {
           // if this variable is set,   make a fake costmap
           make_fake_costmap_ = true;
           } else {
           make_fake_costmap_ = false;
           }
          
           client_ptr_ = rclcpp_action::create_client<Spin>(  
           node_->get_node_base_interface(   ),  
           node_->get_node_graph_interface(   ),  
           node_->get_node_logging_interface(   ),  
           node_->get_node_waitables_interface(   ),  
           "spin" );
          
           publisher_ =
           node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  "initialpose",   10 );
           fake_costmap_publisher_ =
           node_->create_publisher<nav2_msgs::msg::Costmap>(  
           "local_costmap/costmap_raw",  
           rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ) );
           fake_footprint_publisher_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>(  
           "local_costmap/published_footprint",   rclcpp::SystemDefaultsQoS(   ) );
          
           subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &SpinBehaviorTester::amclPoseCallback,   this,   std::placeholders::_1 ) );
          
           stamp_ = node_->now(   );
          }
          
      73  SpinBehaviorTester::~SpinBehaviorTester(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
      80  void SpinBehaviorTester::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           return;
           }
           if (  !make_fake_costmap_ ) {
           while (  !initial_pose_received_ ) {
           RCLCPP_WARN(  node_->get_logger(   ),   "Initial pose not received" );
           sendInitialPose(   );
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node_ );
           }
           } else {
           sendFakeOdom(  0.0 );
           }
          
           // Wait for lifecycle_manager_navigation to activate behavior_server
           std::this_thread::sleep_for(  10s );
          
           if (  !client_ptr_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action client not initialized" );
           is_active_ = false;
           return;
           }
          
           if (  !client_ptr_->wait_for_action_server(  10s ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action server not available after waiting" );
           is_active_ = false;
           return;
           }
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Spin action server is ready" );
           is_active_ = true;
          }
          
     116  void SpinBehaviorTester::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
           is_active_ = false;
          }
          
     124  bool SpinBehaviorTester::defaultSpinBehaviorTest(  
           const float target_yaw,  
           const double tolerance )
          {
           if (  !is_active_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Not activated" );
           return false;
           }
          
           // Sleep to let behavior server be ready for serving in multiple runs
           std::this_thread::sleep_for(  5s );
          
           if (  make_fake_costmap_ ) {
           sendFakeOdom(  0.0 );
           }
          
           auto goal_msg = Spin::Goal(   );
           goal_msg.target_yaw = target_yaw;
          
           // Intialize fake costmap
           if (  make_fake_costmap_ ) {
           sendFakeCostmap(  target_yaw );
           sendFakeOdom(  0.0 );
           }
          
           geometry_msgs::msg::PoseStamped initial_pose;
           if (  !nav2_util::getCurrentPose(  initial_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
           RCLCPP_INFO(  node_->get_logger(   ),   "Found current robot pose" );
           RCLCPP_INFO(  
           node_->get_logger(   ),  
           "Init Yaw is %lf",  
           fabs(  tf2::getYaw(  initial_pose.pose.orientation ) ) );
           RCLCPP_INFO(  node_->get_logger(   ),   "Before sending goal" );
          
           // Intialize fake costmap
           if (  make_fake_costmap_ ) {
           sendFakeCostmap(  target_yaw );
           sendFakeOdom(  0.0 );
           }
          
           rclcpp::sleep_for(  std::chrono::milliseconds(  100 ) );
          
           auto goal_handle_future = client_ptr_->async_send_goal(  goal_msg );
          
           if (  rclcpp::spin_until_future_complete(  node_,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "send goal call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<Spin>::SharedPtr goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Goal was rejected by server" );
           return false;
           }
          
           // Wait for the server to be done with the goal
           auto result_future = client_ptr_->async_get_result(  goal_handle );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for result" );
           rclcpp::sleep_for(  std::chrono::milliseconds(  1000 ) );
          
           if (  make_fake_costmap_ ) { // if we are faking the costmap,   we will fake success.
           sendFakeOdom(  0.0 );
           sendFakeCostmap(  target_yaw );
           RCLCPP_INFO(  node_->get_logger(   ),   "target_yaw %lf",   target_yaw );
           // Slowly increment command yaw by increment to simulate the robot slowly spinning into place
           float step_size = tolerance / 4.0;
           for (  float command_yaw = 0.0;
           abs(  command_yaw ) < abs(  target_yaw );
           command_yaw = command_yaw + step_size )
           {
           sendFakeOdom(  command_yaw );
           sendFakeCostmap(  target_yaw );
           rclcpp::sleep_for(  std::chrono::milliseconds(  1 ) );
           }
           sendFakeOdom(  target_yaw );
           sendFakeCostmap(  target_yaw );
           RCLCPP_INFO(  node_->get_logger(   ),   "After sending goal" );
           }
           if (  rclcpp::spin_until_future_complete(  node_,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "get result call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<Spin>::WrappedResult wrapped_result = result_future.get(   );
          
           switch (  wrapped_result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED: break;
           case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was aborted" );
           return false;
           case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was canceled" );
           return false;
           default: RCLCPP_ERROR(  node_->get_logger(   ),   "Unknown result code" );
           return false;
           }
          
           RCLCPP_INFO(  node_->get_logger(   ),   "result received" );
          
           geometry_msgs::msg::PoseStamped current_pose;
           if (  !nav2_util::getCurrentPose(  current_pose,   *tf_buffer_,   "odom" ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Current robot pose is not available." );
           return false;
           }
          
           double goal_yaw = angles::normalize_angle(  
           tf2::getYaw(  initial_pose.pose.orientation ) + target_yaw );
           double dyaw = angles::shortest_angular_distance(  
           goal_yaw,   tf2::getYaw(  current_pose.pose.orientation ) );
          
           if (  fabs(  dyaw ) > tolerance ) {
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Init Yaw is %lf (  tolerance %lf )",  
           fabs(  tf2::getYaw(  initial_pose.pose.orientation ) ),   tolerance );
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Current Yaw is %lf (  tolerance %lf )",  
           fabs(  tf2::getYaw(  current_pose.pose.orientation ) ),   tolerance );
           RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Angular distance from goal is %lf (  tolerance %lf )",  
           fabs(  dyaw ),   tolerance );
           return false;
           }
          
           return true;
          }
          
     263  void SpinBehaviorTester::sendFakeCostmap(  float angle )
          {
           nav2_msgs::msg::Costmap fake_costmap;
          
           fake_costmap.header.frame_id = "odom";
           fake_costmap.header.stamp = stamp_;
           fake_costmap.metadata.layer = "master";
           fake_costmap.metadata.resolution = .1;
           fake_costmap.metadata.size_x = 100;
           fake_costmap.metadata.size_y = 100;
           fake_costmap.metadata.origin.position.x = 0;
           fake_costmap.metadata.origin.position.y = 0;
           fake_costmap.metadata.origin.orientation.w = 1.0;
           float costmap_val = 0;
           for (  int ix = 0; ix < 100; ix++ ) {
           for (  int iy = 0; iy < 100; iy++ ) {
           if (  abs(  angle ) > M_PI_2f32 ) {
           // fake obstacles in the way so we get failure due to potential collision
           costmap_val = 100;
           }
           fake_costmap.data.push_back(  costmap_val );
           }
           }
           fake_costmap_publisher_->publish(  fake_costmap );
          }
          
     289  void SpinBehaviorTester::sendInitialPose(   )
          {
           geometry_msgs::msg::PoseWithCovarianceStamped pose;
           pose.header.frame_id = "map";
           pose.header.stamp = stamp_;
           pose.pose.pose.position.x = -2.0;
           pose.pose.pose.position.y = -0.5;
           pose.pose.pose.position.z = 0.0;
           pose.pose.pose.orientation.x = 0.0;
           pose.pose.pose.orientation.y = 0.0;
           pose.pose.pose.orientation.z = 0.0;
           pose.pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 35; i++ ) {
           pose.pose.covariance[i] = 0.0;
           }
           pose.pose.covariance[0] = 0.08;
           pose.pose.covariance[7] = 0.08;
           pose.pose.covariance[35] = 0.05;
          
           publisher_->publish(  pose );
           RCLCPP_INFO(  node_->get_logger(   ),   "Sent initial pose" );
          }
          
     312  void SpinBehaviorTester::sendFakeOdom(  float angle )
          {
           geometry_msgs::msg::TransformStamped transformStamped;
          
           transformStamped.header.stamp = stamp_;
           transformStamped.header.frame_id = "odom";
           transformStamped.child_frame_id = "base_link";
           transformStamped.transform.translation.x = 0.0;
           transformStamped.transform.translation.y = 0.0;
           transformStamped.transform.translation.z = 0.0;
           tf2::Quaternion q;
           q.setRPY(  0,   0,   angle );
           transformStamped.transform.rotation.x = q.x(   );
           transformStamped.transform.rotation.y = q.y(   );
           transformStamped.transform.rotation.z = q.z(   );
           transformStamped.transform.rotation.w = q.w(   );
          
           tf_broadcaster_->sendTransform(  transformStamped );
          
           geometry_msgs::msg::PolygonStamped footprint;
           footprint.header.frame_id = "odom";
           footprint.header.stamp = stamp_;
           footprint.polygon.points.resize(  4 );
           footprint.polygon.points[0].x = 0.22;
           footprint.polygon.points[0].y = 0.22;
           footprint.polygon.points[1].x = 0.22;
           footprint.polygon.points[1].y = -0.22;
           footprint.polygon.points[2].x = -0.22;
           footprint.polygon.points[2].y = -0.22;
           footprint.polygon.points[3].x = -0.22;
           footprint.polygon.points[3].y = 0.22;
           fake_footprint_publisher_->publish(  footprint );
          }
     345  void SpinBehaviorTester::amclPoseCallback(  
     346   const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
          {
           initial_pose_received_ = true;
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp

          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
          #define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <thread>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "angles/angles.h"
          #include "nav2_msgs/action/spin.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/point32.hpp"
          #include "geometry_msgs/msg/polygon_stamped.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
          #include "geometry_msgs/msg/transform_stamped.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          
          #include "tf2/utils.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_broadcaster.h"
          #include "tf2_ros/transform_listener.h"
          
          namespace nav2_system_tests
          {
          
      48  class SpinBehaviorTester
          {
          public:
           using Spin = nav2_msgs::action::Spin;
           using GoalHandleSpin = rclcpp_action::ClientGoalHandle<Spin>;
          
           SpinBehaviorTester(   );
           ~SpinBehaviorTester(   );
          
           // Runs a single test with given target yaw
           bool defaultSpinBehaviorTest(  
           float target_yaw,  
           double tolerance = 0.1 );
          
           void activate(   );
          
           void deactivate(   );
          
           bool isActive(   ) const
           {
           return is_active_;
           }
          
          private:
           void sendInitialPose(   );
          
      74   void sendFakeCostmap(  float angle );
      75   void sendFakeOdom(  float angle );
          
      77   void amclPoseCallback(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
          
           bool is_active_;
           bool initial_pose_received_;
           bool make_fake_costmap_;
           rclcpp::Time stamp_;
          
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
           std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
          
           rclcpp::Node::SharedPtr node_;
          
           // Publisher to publish initial pose
           rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
          
           // Publisher to publish fake costmap raw
           rclcpp::Publisher<nav2_msgs::msg::Costmap>::SharedPtr fake_costmap_publisher_;
          
           // Publisher to publish fake costmap footprint
           rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr fake_footprint_publisher_;
          
           // Subscriber for amcl pose
           rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
          
           // Action client to call spin action
           rclcpp_action::Client<Spin>::SharedPtr client_ptr_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp

          // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <cmath>
          #include <tuple>
          #include <string>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "spin_behavior_tester.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::SpinBehaviorTester;
          
      29  std::string testNameGenerator(  const testing::TestParamInfo<std::tuple<float,   float>> & param )
          {
           std::string name = std::to_string(  std::abs(  std::get<0>(  param.param ) ) ) + "_" + std::to_string(  
           std::get<1>(  param.param ) );
           name.erase(  std::remove(  name.begin(   ),   name.end(   ),   '.' ),   name.end(   ) );
           return name;
          }
          
      37  class SpinBehaviorTestFixture
      38   : public ::testing::TestWithParam<std::tuple<float,   float>>
          {
          public:
           static void SetUpTestCase(   )
           {
           spin_recovery_tester = new SpinBehaviorTester(   );
           if (  !spin_recovery_tester->isActive(   ) ) {
           spin_recovery_tester->activate(   );
           }
           }
          
           static void TearDownTestCase(   )
           {
           delete spin_recovery_tester;
           spin_recovery_tester = nullptr;
           }
          
          protected:
           static SpinBehaviorTester * spin_recovery_tester;
          };
          
          SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr;
          
          TEST_P(  SpinBehaviorTestFixture,   testSpinRecovery )
          {
           float target_yaw = std::get<0>(  GetParam(   ) );
           float tolerance = std::get<1>(  GetParam(   ) );
          
           bool success = false;
           int num_tries = 3;
           for (  int i = 0; i != num_tries; i++ ) {
           success = success || spin_recovery_tester->defaultSpinBehaviorTest(  target_yaw,   tolerance );
           if (  success ) {
           break;
           }
           }
           if (  std::getenv(  "MAKE_FAKE_COSTMAP" ) != NULL && abs(  target_yaw ) > M_PI_2f32 ) {
           // if this variable is set,   make a fake costmap
           // in the fake spin test,   we expect a collision for angles > M_PI_2
      77   EXPECT_EQ(  false,   success );
           } else {
           EXPECT_EQ(  true,   success );
           }
          }
          
          INSTANTIATE_TEST_SUITE_P(  
           SpinRecoveryTests,  
           SpinBehaviorTestFixture,  
           ::testing::Values(  
           std::make_tuple(  -M_PIf32 / 6.0,   0.1 ),  
           std::make_tuple(  M_PI_4f32,   0.1 ),  
           std::make_tuple(  -M_PI_2f32,   0.1 ),  
           std::make_tuple(  M_PIf32,   0.1 ),  
           std::make_tuple(  3.0 * M_PIf32 / 2.0,   0.15 ),  
           std::make_tuple(  -2.0 * M_PIf32,   0.1 ),  
           std::make_tuple(  4.0 * M_PIf32,   0.15 ) ),  
           testNameGenerator );
          
          int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <cmath>
          #include <tuple>
          #include <string>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          
          #include "wait_behavior_tester.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::WaitBehaviorTester;
          
      30  std::string testNameGenerator(  const testing::TestParamInfo<std::tuple<float,   float>> & param )
          {
           std::string name = std::to_string(  std::abs(  std::get<0>(  param.param ) ) ) + "_" + std::to_string(  
           std::get<1>(  param.param ) );
           name.erase(  std::remove(  name.begin(   ),   name.end(   ),   '.' ),   name.end(   ) );
           return name;
          }
          
      38  class WaitBehaviorTestFixture
      39   : public ::testing::TestWithParam<std::tuple<float,   float>>
          {
          public:
           static void SetUpTestCase(   )
           {
           wait_behavior_tester = new WaitBehaviorTester(   );
           if (  !wait_behavior_tester->isActive(   ) ) {
           wait_behavior_tester->activate(   );
           }
           }
          
           static void TearDownTestCase(   )
           {
           delete wait_behavior_tester;
           wait_behavior_tester = nullptr;
           }
          
          protected:
           static WaitBehaviorTester * wait_behavior_tester;
          };
          
          WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr;
          
      62  TEST_P(  WaitBehaviorTestFixture,   testSWaitBehavior )
          {
           float wait_time = std::get<0>(  GetParam(   ) );
           float cancel = std::get<1>(  GetParam(   ) );
          
           bool success = false;
           int num_tries = 3;
           for (  int i = 0; i != num_tries; i++ ) {
           if (  cancel == 1.0 ) {
           success = success || wait_behavior_tester->behaviorTestCancel(  wait_time );
           } else {
           success = success || wait_behavior_tester->behaviorTest(  wait_time );
           }
           if (  success ) {
           break;
           }
           }
          
           EXPECT_EQ(  true,   success );
          }
          
      83  INSTANTIATE_TEST_SUITE_P(  
           WaitBehaviorTests,  
           WaitBehaviorTestFixture,  
           ::testing::Values(  
           std::make_tuple(  1.0,   0.0 ),  
           std::make_tuple(  2.0,   0.0 ),  
           std::make_tuple(  5.0,   0.0 ),  
           std::make_tuple(  10.0,   1.0 ) ),  
           testNameGenerator );
          
      93  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <random>
          #include <tuple>
          #include <memory>
          #include <iostream>
          #include <chrono>
          #include <sstream>
          #include <iomanip>
          
          #include "wait_behavior_tester.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          
          namespace nav2_system_tests
          {
          
      34  WaitBehaviorTester::WaitBehaviorTester(   )
          : is_active_(  false ),  
           initial_pose_received_(  false )
          {
           node_ = rclcpp::Node::make_shared(  "wait_behavior_test" );
          
           tf_buffer_ = std::make_shared<tf2_ros::Buffer>(  node_->get_clock(   ) );
           tf_listener_ = std::make_shared<tf2_ros::TransformListener>(  *tf_buffer_ );
          
           client_ptr_ = rclcpp_action::create_client<Wait>(  
           node_->get_node_base_interface(   ),  
           node_->get_node_graph_interface(   ),  
           node_->get_node_logging_interface(   ),  
           node_->get_node_waitables_interface(   ),  
           "wait" );
          
           publisher_ =
           node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  "initialpose",   10 );
          
           subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &WaitBehaviorTester::amclPoseCallback,   this,   std::placeholders::_1 ) );
          }
          
      58  WaitBehaviorTester::~WaitBehaviorTester(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
      65  void WaitBehaviorTester::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           return;
           }
          
           while (  !initial_pose_received_ ) {
           RCLCPP_WARN(  node_->get_logger(   ),   "Initial pose not received" );
           sendInitialPose(   );
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node_ );
           }
          
           // Wait for lifecycle_manager_navigation to activate behavior_server
           std::this_thread::sleep_for(  10s );
          
           if (  !client_ptr_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action client not initialized" );
           is_active_ = false;
           return;
           }
          
           if (  !client_ptr_->wait_for_action_server(  10s ) ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Action server not available after waiting" );
           is_active_ = false;
           return;
           }
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Wait action server is ready" );
           is_active_ = true;
          }
          
      98  void WaitBehaviorTester::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           }
           is_active_ = false;
          }
          
     106  bool WaitBehaviorTester::behaviorTest(  
           const float wait_time )
          {
           if (  !is_active_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Not activated" );
           return false;
           }
          
           // Sleep to let behavior server be ready for serving in multiple runs
           std::this_thread::sleep_for(  5s );
          
           auto start_time = node_->now(   );
           auto goal_msg = Wait::Goal(   );
           goal_msg.time = rclcpp::Duration(  wait_time,   0.0 );
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Sending goal" );
          
           auto goal_handle_future = client_ptr_->async_send_goal(  goal_msg );
          
           if (  rclcpp::spin_until_future_complete(  node_,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "send goal call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Goal was rejected by server" );
           return false;
           }
          
           // Wait for the server to be done with the goal
           auto result_future = client_ptr_->async_get_result(  goal_handle );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for result" );
           if (  rclcpp::spin_until_future_complete(  node_,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "get result call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<Wait>::WrappedResult wrapped_result = result_future.get(   );
          
           switch (  wrapped_result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED: break;
           case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was aborted" );
           return false;
           case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was canceled" );
           return false;
           default: RCLCPP_ERROR(  node_->get_logger(   ),   "Unknown result code" );
           return false;
           }
          
           RCLCPP_INFO(  node_->get_logger(   ),   "result received" );
          
          
           if (  (  node_->now(   ) - start_time ).seconds(   ) < static_cast<double>(  wait_time ) ) {
           return false;
           }
          
           return true;
          }
          
     175  bool WaitBehaviorTester::behaviorTestCancel(  
           const float wait_time )
          {
           if (  !is_active_ ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Not activated" );
           return false;
           }
          
           // Sleep to let behavior server be ready for serving in multiple runs
           std::this_thread::sleep_for(  5s );
          
           auto start_time = node_->now(   );
           auto goal_msg = Wait::Goal(   );
           goal_msg.time = rclcpp::Duration(  wait_time,   0.0 );
          
           RCLCPP_INFO(  this->node_->get_logger(   ),   "Sending goal" );
          
           auto goal_handle_future = client_ptr_->async_send_goal(  goal_msg );
          
           if (  rclcpp::spin_until_future_complete(  node_,   goal_handle_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "send goal call failed :(  " );
           return false;
           }
          
           rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get(   );
           if (  !goal_handle ) {
           RCLCPP_ERROR(  node_->get_logger(   ),   "Goal was rejected by server" );
           return false;
           }
          
           // Wait for the server to be done with the goal
           auto result_future = client_ptr_->async_cancel_all_goals(   );
          
           RCLCPP_INFO(  node_->get_logger(   ),   "Waiting for cancellation" );
           if (  rclcpp::spin_until_future_complete(  node_,   result_future ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           RCLCPP_ERROR(  node_->get_logger(   ),   "get cancel result call failed :(  " );
           return false;
           }
          
           auto status = goal_handle_future.get(   )->get_status(   );
          
           switch (  status ) {
           case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal succeeded" );
           return false;
           case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal was aborted" );
           return false;
           case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO(  
           node_->get_logger(   ),  
           "Goal was canceled" );
           return true;
           case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO(  
           node_->get_logger(   ),  
           "Goal is cancelling" );
           return true;
           case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal is executing" );
           return false;
           case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR(  
           node_->get_logger(   ),  
           "Goal is processing" );
           return false;
           default: RCLCPP_ERROR(  node_->get_logger(   ),   "Unknown result code" );
           return false;
           }
          
           return false;
          }
          
     252  void WaitBehaviorTester::sendInitialPose(   )
          {
           geometry_msgs::msg::PoseWithCovarianceStamped pose;
           pose.header.frame_id = "map";
           pose.header.stamp = rclcpp::Time(   );
           pose.pose.pose.position.x = -2.0;
           pose.pose.pose.position.y = -0.5;
           pose.pose.pose.position.z = 0.0;
           pose.pose.pose.orientation.x = 0.0;
           pose.pose.pose.orientation.y = 0.0;
           pose.pose.pose.orientation.z = 0.0;
           pose.pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 35; i++ ) {
           pose.pose.covariance[i] = 0.0;
           }
           pose.pose.covariance[0] = 0.08;
           pose.pose.covariance[7] = 0.08;
           pose.pose.covariance[35] = 0.05;
          
           publisher_->publish(  pose );
           RCLCPP_INFO(  node_->get_logger(   ),   "Sent initial pose" );
          }
          
     275  void WaitBehaviorTester::amclPoseCallback(  
     276   const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
          {
           initial_pose_received_ = true;
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp

          // Copyright (  c ) 2020 Samsung Research
          // Copyright (  c ) 2020 Sarthak Mittal
          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
          #define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <thread>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "angles/angles.h"
          #include "nav2_msgs/action/wait.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
          
          #include "tf2/utils.h"
          #include "tf2_ros/buffer.h"
          #include "tf2_ros/transform_listener.h"
          
          namespace nav2_system_tests
          {
          
      42  class WaitBehaviorTester
          {
          public:
           using Wait = nav2_msgs::action::Wait;
           using GoalHandleWait = rclcpp_action::ClientGoalHandle<Wait>;
          
           WaitBehaviorTester(   );
           ~WaitBehaviorTester(   );
          
           // Runs a single test with given target yaw
           bool behaviorTest(  
           float time );
          
           bool behaviorTestCancel(  float time );
          
           void activate(   );
          
           void deactivate(   );
          
           bool isActive(   ) const
           {
           return is_active_;
           }
          
          private:
           void sendInitialPose(   );
          
      69   void amclPoseCallback(  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
          
           bool is_active_;
           bool initial_pose_received_;
          
           std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
           std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
          
           rclcpp::Node::SharedPtr node_;
          
           // Publisher to publish initial pose
           rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
          
           // Subscriber for amcl pose
           rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
          
           // Action client to call wait action
           rclcpp_action::Client<Wait>::SharedPtr client_ptr_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <ctime>
          #include <thread>
          #include <memory>
          #include <utility>
          
          #include "dummy_controller.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_system_tests
          {
          
      28  DummyController::DummyController(   )
          : Node(  "DummyController" )
          {
           RCLCPP_INFO(  get_logger(   ),   "Initializing DummyController..." );
          
           auto temp_node = std::shared_ptr<rclcpp::Node>(  this,   [](  auto ) {} );
          
           vel_pub_ =
           this->create_publisher<geometry_msgs::msg::Twist>(  "cmd_vel",   1 );
          
           task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>(  temp_node,   false ),  
           task_server_->setExecuteCallback(  
           std::bind(  &DummyController::followPath,   this,   std::placeholders::_1 ) );
          
           // Start listening for incoming ComputePathToPose action server requests
           task_server_->start(   );
          
           RCLCPP_INFO(  get_logger(   ),   "Initialized DummyController" );
          }
          
      48  DummyController::~DummyController(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down DummyController" );
          }
          
          void
      54  DummyController::followPath(  const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Starting controller " );
          
           auto start_time = std::chrono::system_clock::now(   );
           auto time_since_msg = std::chrono::system_clock::now(   );
          
           while (  true ) {
           // Dummy controller computation time
           std::this_thread::sleep_for(  50ms );
          
           if (  task_server_->cancelRequested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Task cancelled" );
           setZeroVelocity(   );
           task_server_->setCanceled(   );
           return;
           }
          
           // Log a message every second
           auto current_time = std::chrono::system_clock::now(   );
           if (  current_time - time_since_msg >= 1s ) {
           RCLCPP_INFO(  get_logger(   ),   "Following path" );
           time_since_msg = std::chrono::system_clock::now(   );
           }
          
           // Output control command
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
           cmd_vel->linear.x = 0.1;
           vel_pub_->publish(  std::move(  cmd_vel ) );
          
           if (  current_time - start_time >= 30s ) {
           RCLCPP_INFO(  get_logger(   ),   "Reached end point" );
           setZeroVelocity(   );
           break;
           }
           }
          
           nav2_behavior_tree::FollowPathResult result;
           task_server_->setResult(  result );
          }
          
      95  void DummyController::setZeroVelocity(   )
          {
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
           cmd_vel->linear.x = 0.0;
           cmd_vel->linear.y = 0.0;
           cmd_vel->angular.z = 0.0;
           vel_pub_->publish(  std::move(  cmd_vel ) );
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
          #define DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
          
          #include <memory>
          
          #include "nav2_behavior_tree/follow_path_task.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          
          namespace nav2_system_tests
          {
          
      26  class DummyController : public rclcpp::Node
          {
          public:
      29   DummyController(   );
      30   ~DummyController(   );
          
      32   nav2_behavior_tree::TaskStatus followPath(  
      33   const nav2_behavior_tree::FollowPathCommand::SharedPtr command );
          
          private:
      36   void setZeroVelocity(   );
          
      38   std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
          
           std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_

navigation2/nav2_system_tests/src/dummy_controller/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "dummy_controller.hpp"
          
      19  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           rclcpp::spin(  std::make_shared<nav2_system_tests::DummyController>(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_system_tests/src/dummy_planner/dummy_planner.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <thread>
          #include <memory>
          
          #include "dummy_planner.hpp"
          
          using namespace std::chrono_literals;
          
          namespace nav2_system_tests
          {
          
      26  DummyPlanner::DummyPlanner(   )
          : Node(  "DummyPlanner" )
          {
           RCLCPP_INFO(  get_logger(   ),   "Initializing DummyPlanner..." );
          
           auto temp_node = std::shared_ptr<rclcpp::Node>(  this,   [](  auto ) {} );
          
           task_server_ =
           std::make_unique<nav2_behavior_tree::ComputePathToPoseTaskServer>(  temp_node,   false ),  
           task_server_->setExecuteCallback(  
           std::bind(  &DummyPlanner::computePlan,   this,   std::placeholders::_1 ) );
          
           // Start listening for incoming ComputePathToPose task requests
           task_server_->start(   );
          
           RCLCPP_INFO(  get_logger(   ),   "Initialized DummyPlanner" );
          }
          
      44  DummyPlanner::~DummyPlanner(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down DummyPlanner" );
          }
          
          void
      50  DummyPlanner::computePlan(  const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr cmd )
          {
           RCLCPP_INFO(  
           get_logger(   ),   "Attempting to a find path from (  %.2f,   %.2f ) to "
           "(  %.2f,   %.2f ).",   cmd->start.position.x,   cmd->start.position.y,  
           cmd->goal.position.x,   cmd->goal.position.y );
          
           // Dummy path computation time
           std::this_thread::sleep_for(  500ms );
          
           if (  task_server_->cancelRequested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Cancelled planning task." );
           task_server_->setCanceled(   );
           return;
           }
          
           RCLCPP_INFO(  get_logger(   ),   "Found a dummy path" );
          
           nav2_behavior_tree::ComputePathToPoseResult result;
           // set succeeded
           task_server_->setResult(  result );
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/dummy_planner/dummy_planner.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef DUMMY_PLANNER__DUMMY_PLANNER_HPP_
          #define DUMMY_PLANNER__DUMMY_PLANNER_HPP_
          
          #include <memory>
          
          #include "nav2_behavior_tree/compute_path_to_pose_task.hpp"
          
          namespace nav2_system_tests
          {
          
      25  class DummyPlanner : public rclcpp::Node
          {
          public:
      28   DummyPlanner(   );
      29   ~DummyPlanner(   );
          
      31   nav2_behavior_tree::TaskStatus computePathToPose(  
      32   const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr command );
          
          private:
      35   std::unique_ptr<nav2_behavior_tree::ComputePathToPoseTaskServer> task_server_;
          };
          
          } // namespace nav2_system_tests
          
          #endif // DUMMY_PLANNER__DUMMY_PLANNER_HPP_

navigation2/nav2_system_tests/src/dummy_planner/main.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "dummy_planner.hpp"
          
      19  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           rclcpp::spin(  std::make_shared<nav2_system_tests::DummyPlanner>(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_system_tests/src/localization/test_localization_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_amcl/amcl_node.hpp"
          #include "std_msgs/msg/string.hpp"
          #include "geometry_msgs/msg/pose_array.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          
          using std::placeholders::_1;
          using namespace std::chrono_literals;
          
          // rclcpp::init can only be called once per process,   so this needs to be a global variable
      27  class RclCppFixture
          {
          public:
      30   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      31   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      35  class TestAmclPose : public ::testing::Test
          {
          public:
      38   TestAmclPose(   )
           {
           pose_callback_ = false;
           initTestPose(   );
           tol_ = 0.25;
          
           node = rclcpp::Node::make_shared(  "localization_test" );
          
           while (  node->count_subscribers(  "scan" ) < 1 ) {
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node );
           }
          
           initial_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "initialpose",   rclcpp::SystemDefaultsQoS(   ) );
           subscription_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(  
           "amcl_pose",   rclcpp::QoS(  rclcpp::KeepLast(  1 ) ).transient_local(   ).reliable(   ),  
           std::bind(  &TestAmclPose::amcl_pose_callback,   this,   _1 ) );
           initial_pose_pub_->publish(  testPose_ );
           }
          
      59   bool defaultAmclTest(   );
          
          protected:
      62   std::shared_ptr<rclcpp::Node> node;
      63   void initTestPose(   );
          
          private:
      66   void amcl_pose_callback(  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg )
           {
           auto amcl_pose = msg->pose;
           amcl_pose_x = amcl_pose.pose.position.x;
           amcl_pose_y = amcl_pose.pose.position.y;
           pose_callback_ = true;
           }
      73   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
      74   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
      75   geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
           double amcl_pose_x;
           double amcl_pose_y;
      78   bool pose_callback_;
           float tol_;
          };
          
      82  bool TestAmclPose::defaultAmclTest(   )
          {
           initial_pose_pub_->publish(  testPose_ );
           while (  !pose_callback_ ) {
           // TODO(  mhpanah ): Initial pose should only be published once.
           initial_pose_pub_->publish(  testPose_ );
           std::this_thread::sleep_for(  1s );
           rclcpp::spin_some(  node );
           }
           if (  std::abs(  amcl_pose_x - testPose_.pose.pose.position.x ) < tol_ &&
           std::abs(  amcl_pose_y - testPose_.pose.pose.position.y ) < tol_ )
           {
           return true;
           } else {
           return false;
           }
          }
          
     100  void TestAmclPose::initTestPose(   )
          {
           testPose_.header.frame_id = "map";
           testPose_.header.stamp = rclcpp::Time(   );
           testPose_.pose.pose.position.x = -2.0;
           testPose_.pose.pose.position.y = -0.5;
           testPose_.pose.pose.position.z = 0.0;
           testPose_.pose.pose.orientation.x = 0.0;
           testPose_.pose.pose.orientation.y = 0.0;
           testPose_.pose.pose.orientation.z = 0.0;
           testPose_.pose.pose.orientation.w = 1.0;
           for (  int i = 0; i < 35; i++ ) {
           testPose_.pose.covariance[i] = 0.0;
           }
           testPose_.pose.covariance[0] = 0.08;
           testPose_.pose.covariance[7] = 0.08;
           testPose_.pose.covariance[35] = 0.05;
          }
          
     119  TEST_F(  TestAmclPose,   SimpleAmclTest )
          {
           EXPECT_EQ(  true,   defaultAmclTest(   ) );
          }

navigation2/nav2_system_tests/src/planning/planner_tester.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <string>
          #include <random>
          #include <tuple>
          #include <utility>
          #include <vector>
          #include <memory>
          #include <iostream>
          #include <chrono>
          #include <sstream>
          #include <iomanip>
          
          #include "planner_tester.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          #include "nav2_map_server/map_mode.hpp"
          #include "nav2_map_server/map_io.hpp"
          #include "nav2_msgs/msg/costmap_meta_data.hpp"
          
          using namespace std::chrono_literals;
          using namespace std::chrono; // NOLINT
          using nav2_util::Costmap;
          using nav2_util::TestCostmap;
          
          namespace nav2_system_tests
          {
          
      40  PlannerTester::PlannerTester(   )
          : Node(  "PlannerTester" ),   is_active_(  false ),  
           map_set_(  false ),   costmap_set_(  false ),  
           using_fake_costmap_(  true ),   trinary_costmap_(  true ),  
           track_unknown_space_(  false ),   lethal_threshold_(  100 ),   unknown_cost_value_(  -1 ),  
           testCostmapType_(  TestCostmap::open_space ),   base_transform_(  nullptr ),  
           map_publish_rate_(  100s )
          {
          }
          
      50  void PlannerTester::activate(   )
          {
           if (  is_active_ ) {
           throw std::runtime_error(  "Trying to activate while already active" );
           return;
           }
           is_active_ = true;
          
           // Launch a thread to process the messages for this node
           spin_thread_ = std::make_unique<nav2_util::NodeThread>(  this );
          
           // We start with a 10x10 grid with no obstacles
           costmap_ = std::make_unique<Costmap>(  this );
           loadSimpleCostmap(  TestCostmap::open_space );
          
           startRobotTransform(   );
          
           // The navfn wrapper
           auto state = rclcpp_lifecycle::State(   );
           planner_tester_ = std::make_shared<NavFnPlannerTester>(   );
           planner_tester_->declare_parameter(  
           "GridBased.use_astar",   rclcpp::ParameterValue(  true ) );
           planner_tester_->set_parameter(  
           rclcpp::Parameter(  std::string(  "GridBased.use_astar" ),   rclcpp::ParameterValue(  true ) ) );
           planner_tester_->set_parameter(  
           rclcpp::Parameter(  std::string(  "expected_planner_frequency" ),   rclcpp::ParameterValue(  -1.0 ) ) );
           planner_tester_->onConfigure(  state );
           publishRobotTransform(   );
           map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(  "map",   1 );
           path_valid_client_ = this->create_client<nav2_msgs::srv::IsPathValid>(  "is_path_valid" );
           rclcpp::Rate r(  1 );
           r.sleep(   );
           planner_tester_->onActivate(  state );
          }
          
      85  void PlannerTester::deactivate(   )
          {
           if (  !is_active_ ) {
           throw std::runtime_error(  "Trying to deactivate while already inactive" );
           return;
           }
           is_active_ = false;
          
           spin_thread_.reset(   );
          
           auto state = rclcpp_lifecycle::State(   );
           planner_tester_->onDeactivate(  state );
           planner_tester_->onCleanup(  state );
          
           map_timer_.reset(   );
           map_pub_.reset(   );
           map_.reset(   );
           tf_broadcaster_.reset(   );
          }
          
     105  PlannerTester::~PlannerTester(   )
          {
           if (  is_active_ ) {
           deactivate(   );
           }
          }
          
     112  void PlannerTester::startRobotTransform(   )
          {
           // Provide the robot pose transform
           tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(  this );
          
           // Set an initial pose
           geometry_msgs::msg::Point robot_position;
           robot_position.x = 1.0;
           robot_position.y = 1.0;
           updateRobotPosition(  robot_position );
          
           // Publish the transform periodically
           transform_timer_ = create_wall_timer(  
           100ms,   std::bind(  &PlannerTester::publishRobotTransform,   this ) );
          }
          
     128  void PlannerTester::updateRobotPosition(  const geometry_msgs::msg::Point & position )
          {
           if (  !base_transform_ ) {
           base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>(   );
           base_transform_->header.frame_id = "map";
           base_transform_->child_frame_id = "base_link";
           }
           std::cout << now(   ).nanoseconds(   ) << std::endl;
          
           base_transform_->header.stamp = now(   ) + rclcpp::Duration(  0.25s );
           base_transform_->transform.translation.x = position.x;
           base_transform_->transform.translation.y = position.y;
           base_transform_->transform.rotation.w = 1.0;
          
           publishRobotTransform(   );
          }
          
     145  void PlannerTester::publishRobotTransform(   )
          {
           if (  base_transform_ ) {
           tf_broadcaster_->sendTransform(  *base_transform_ );
           }
          }
          
     152  void PlannerTester::loadDefaultMap(   )
          {
           // Specs for the default map
           double resolution = 1.0;
           bool negate = false;
           double occupancy_threshold = 0.65;
           double free_threshold = 0.196;
          
           // Define origin offset
           std::vector<double> origin = {0.0,   0.0,   0.0};
          
           nav2_map_server::MapMode mode = nav2_map_server::MapMode::Trinary;
          
           std::string file_path = "";
           char const * path = getenv(  "TEST_MAP" );
           if (  path == NULL ) {
           throw std::runtime_error(  
           "Path to map image file"
           " has not been specified in environment variable `TEST_MAP`." );
           } else {
           file_path = std::string(  path );
           }
          
           RCLCPP_INFO(  this->get_logger(   ),   "Loading map with file_path: %s",   file_path.c_str(   ) );
          
           try {
           map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>(   );
          
           nav2_map_server::LoadParameters load_parameters;
           load_parameters.image_file_name = file_path;
           load_parameters.resolution = resolution;
           load_parameters.origin = origin;
           load_parameters.free_thresh = free_threshold;
           load_parameters.occupied_thresh = occupancy_threshold;
           load_parameters.mode = mode;
           load_parameters.negate = negate;
           loadMapFromFile(  load_parameters,   *map_ );
           } catch (  ... ) {
           RCLCPP_ERROR(  
           this->get_logger(   ),  
           "Failed to load image from file: %s",   file_path.c_str(   ) );
           throw;
           }
          
           map_->header.stamp = this->now(   );
           map_->header.frame_id = "map";
           map_->info.map_load_time = this->now(   );
          
           // TODO(  orduno ): #443 replace with a latched topic
           map_timer_ = create_wall_timer(  1s,   [this](   ) -> void {map_pub_->publish(  *map_ );} );
          
           map_set_ = true;
           costmap_set_ = false;
           using_fake_costmap_ = false;
          
           setCostmap(   );
          }
          
     210  void PlannerTester::loadSimpleCostmap(  const TestCostmap & testCostmapType )
          {
           RCLCPP_INFO(  get_logger(   ),   "loadSimpleCostmap called." );
           if (  costmap_set_ ) {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Setting a new costmap with fake values" );
           }
          
           costmap_->set_test_costmap(  testCostmapType );
          
           costmap_set_ = true;
           using_fake_costmap_ = true;
          }
          
     223  void PlannerTester::setCostmap(   )
          {
           if (  !map_set_ ) {
           RCLCPP_ERROR(  this->get_logger(   ),   "Map has not been provided" );
           return;
           }
          
           costmap_ = std::make_unique<Costmap>(  
           this,   trinary_costmap_,   track_unknown_space_,   lethal_threshold_,   unknown_cost_value_ );
          
           costmap_->set_static_map(  *map_ );
          
           costmap_set_ = true;
           using_fake_costmap_ = false;
          }
          
     239  bool PlannerTester::defaultPlannerTest(  
     240   ComputePathToPoseResult & path,  
           const double /*deviation_tolerance*/ )
          {
           if (  !costmap_set_ ) {
           RCLCPP_ERROR(  this->get_logger(   ),   "Costmap must be set before requesting a plan" );
           return false;
           }
          
           // TODO(  orduno ) #443 Add support for planners that take into account robot orientation
           geometry_msgs::msg::Point robot_position;
           ComputePathToPoseCommand goal;
           auto costmap_properties = costmap_->get_properties(   );
          
           if (  using_fake_costmap_ ) {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Planning using a fake costmap" );
          
           robot_position.x = 1.0;
           robot_position.y = 1.0;
          
           goal.pose.position.x = 8.0;
           goal.pose.position.y = 8.0;
          
           } else {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Planning using the provided map" );
          
           // Defined with respect to world coordinate system
           // Planner will do coordinate transformation to map internally
           robot_position.x = 390.0;
           robot_position.y = 10.0;
          
           goal.pose.position.x = 10.0;
           goal.pose.position.y = 390.0;
           }
          
           // TODO(  orduno ): #443 On a default test,   provide the reference path to compare with the planner
           // result.
           return plannerTest(  robot_position,   goal,   path );
          }
          
     279  bool PlannerTester::defaultPlannerRandomTests(  
           const unsigned int number_tests,  
           const float acceptable_fail_ratio = 0.1 )
          {
           if (  !costmap_set_ ) {
           RCLCPP_ERROR(  this->get_logger(   ),   "Costmap must be set before requesting a plan" );
           return false;
           }
          
           if (  using_fake_costmap_ ) {
           RCLCPP_ERROR(  
           this->get_logger(   ),  
           "Randomized testing with hardcoded costmaps not implemented yet" );
           return false;
           }
          
           // Initialize random number generator
           std::random_device random_device;
           std::mt19937 generator(  random_device(   ) );
          
           // Obtain random positions within map
           std::uniform_int_distribution<> distribution_x(  1,   costmap_->get_properties(   ).size_x - 1 );
           std::uniform_int_distribution<> distribution_y(  1,   costmap_->get_properties(   ).size_y - 1 );
          
           auto generate_random = [&](   ) mutable -> std::pair<int,   int> {
           bool point_is_free = false;
           int x,   y;
           while (  !point_is_free ) {
           x = distribution_x(  generator );
           y = distribution_y(  generator );
           point_is_free = costmap_->is_free(  x,   y );
           }
           return std::make_pair(  x,   y );
           };
          
           // TODO(  orduno ) #443 Add support for planners that take into account robot orientation
           geometry_msgs::msg::Point robot_position;
           ComputePathToPoseCommand goal;
           ComputePathToPoseResult path;
          
           unsigned int num_fail = 0;
           auto start = high_resolution_clock::now(   );
           for (  unsigned int test_num = 0; test_num < number_tests; ++test_num ) {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Running test #%u",   test_num + 1 );
          
           // Compose the robot start position and goal using random numbers
           // Defined with respect to world coordinate system
           // Planner will do coordinate transformation to map internally
          
           auto vals = generate_random(   );
           robot_position.x = vals.first;
           robot_position.y = vals.second;
          
           vals = generate_random(   );
           goal.pose.position.x = vals.first;
           goal.pose.position.y = vals.second;
          
           if (  !plannerTest(  robot_position,   goal,   path ) ) {
           RCLCPP_WARN(  
           this->get_logger(   ),   "Failed with start at %0.2f,   %0.2f and goal at %0.2f,   %0.2f",  
           robot_position.x,   robot_position.y,   goal.pose.position.x,   goal.pose.position.y );
           ++num_fail;
           }
           }
           auto end = high_resolution_clock::now(   );
           auto elapsed = duration_cast<milliseconds>(  end - start );
          
           RCLCPP_INFO(  
           this->get_logger(   ),  
           "Tested with %u tests. Planner failed on %u. Test time %ld ms",  
           number_tests,   num_fail,   elapsed.count(   ) );
          
           if (  (  num_fail / number_tests ) > acceptable_fail_ratio ) {
           return false;
           }
          
           return true;
          }
          
     358  bool PlannerTester::plannerTest(  
     359   const geometry_msgs::msg::Point & robot_position,  
     360   const ComputePathToPoseCommand & goal,  
     361   ComputePathToPoseResult & path )
          {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Getting the path from the planner" );
          
           // First make available the current robot position for the planner to take as starting point
           updateRobotPosition(  robot_position );
           sleep(  0.05 );
          
           // Then request to compute a path
           TaskStatus status = createPlan(  goal,   path );
          
           RCLCPP_DEBUG(  this->get_logger(   ),   "Path request status: %d",   static_cast<int8_t>(  status ) );
          
           if (  status == TaskStatus::FAILED ) {
           return false;
           } else if (  status == TaskStatus::SUCCEEDED ) {
           // TODO(  orduno ): #443 check why task may report success while planner returns a path of 0 points
           RCLCPP_DEBUG(  this->get_logger(   ),   "Got path,   checking for possible collisions" );
           return isCollisionFree(  path ) && isWithinTolerance(  robot_position,   goal,   path );
           }
          
           return false;
          }
          
     385  TaskStatus PlannerTester::createPlan(  
     386   const ComputePathToPoseCommand & goal,  
     387   ComputePathToPoseResult & path )
          {
           // Update the costmap of the planner to the set data
           planner_tester_->setCostmap(  costmap_.get(   ) );
          
           // Call planning algorithm
           if (  planner_tester_->createPath(  goal,   path ) ) {
           return TaskStatus::SUCCEEDED;
           }
          
           return TaskStatus::FAILED;
          }
          
     400  bool PlannerTester::isPathValid(  nav_msgs::msg::Path & path )
          {
           planner_tester_->setCostmap(  costmap_.get(   ) );
           // create a fake service request
           auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>(   );
           request->path = path;
           auto result = path_valid_client_->async_send_request(  request );
          
           RCLCPP_INFO(  this->get_logger(   ),   "Waiting for service complete" );
           if (  rclcpp::spin_until_future_complete(  
           this->planner_tester_,   result,  
           std::chrono::milliseconds(  100 ) ) ==
           rclcpp::FutureReturnCode::SUCCESS )
           {
           return result.get(   )->is_valid;
           } else {
           RCLCPP_INFO(  get_logger(   ),   "Failed to call is_path_valid service" );
           return false;
           }
          }
          
     421  bool PlannerTester::isCollisionFree(  const ComputePathToPoseResult & path )
          {
           // At each point of the path,   check if the corresponding cell is free
          
           // TODO(  orduno ): #443 for now we are assuming the robot is the size of a single cell
           // costmap/world_model has consider the robot footprint
          
           // TODO(  orduno ): #443 Tweak criteria for defining if a path goes into obstacles.
           // Current navfn planner will sometimes produce paths that cut corners
           // i.e. some points are around the corner are actually inside the obstacle
          
           bool collisionFree = true;
          
           for (  auto pose : path.poses ) {
           collisionFree = costmap_->is_free(  
           static_cast<unsigned int>(  std::round(  pose.pose.position.x ) ),  
           static_cast<unsigned int>(  std::round(  pose.pose.position.y ) ) );
          
           if (  !collisionFree ) {
           RCLCPP_WARN(  
           this->get_logger(   ),   "Path has collision at (  %.2f,   %.2f )",  
           pose.pose.position.x,   pose.pose.position.y );
           printPath(  path );
           return false;
           }
           }
          
           RCLCPP_DEBUG(  this->get_logger(   ),   "Path has no collisions" );
           return true;
          }
          
     452  bool PlannerTester::isWithinTolerance(  
     453   const geometry_msgs::msg::Point & robot_position,  
     454   const ComputePathToPoseCommand & goal,  
     455   const ComputePathToPoseResult & path ) const
          {
           return isWithinTolerance(  
           robot_position,   goal,   path,   0.0,   ComputePathToPoseResult(   ) );
          }
          
     461  bool PlannerTester::isWithinTolerance(  
     462   const geometry_msgs::msg::Point & robot_position,  
     463   const ComputePathToPoseCommand & goal,  
     464   const ComputePathToPoseResult & path,  
           const double /*deviationTolerance*/,  
     466   const ComputePathToPoseResult & /*reference_path*/ ) const
          {
           // TODO(  orduno ) #443 Work in progress,   for now we only check that the path start matches the
           // robot start location and that the path end matches the goal.
          
           auto path_start = path.poses[0];
           auto path_end = path.poses.end(   )[-1];
          
           if (  
           path_start.pose.position.x == robot_position.x &&
           path_start.pose.position.y == robot_position.y &&
           path_end.pose.position.x == goal.pose.position.x &&
           path_end.pose.position.y == goal.pose.position.y )
           {
           RCLCPP_DEBUG(  this->get_logger(   ),   "Path has correct start and end points" );
          
           return true;
           }
           RCLCPP_WARN(  this->get_logger(   ),   "Path deviates from requested start and end points" );
          
           RCLCPP_DEBUG(  
           this->get_logger(   ),   "Requested path starts at (  %.2f,   %.2f ) and ends at (  %.2f,   %.2f )",  
           robot_position.x,   robot_position.y,   goal.pose.position.x,   goal.pose.position.y );
          
           RCLCPP_DEBUG(  
           this->get_logger(   ),   "Computed path starts at (  %.2f,   %.2f ) and ends at (  %.2f,   %.2f )",  
           path_start.pose.position.x,   path_start.pose.position.y,  
           path_end.pose.position.x,   path_end.pose.position.y );
          
           return false;
          }
          
     498  void PlannerTester::printPath(  const ComputePathToPoseResult & path ) const
          {
           auto index = 0;
           auto ss = std::stringstream{};
          
           for (  auto pose : path.poses ) {
           ss << " point #" << index << " with" <<
           " x: " << std::setprecision(  3 ) << pose.pose.position.x <<
           " y: " << std::setprecision(  3 ) << pose.pose.position.y << '\n';
           ++index;
           }
          
           RCLCPP_INFO(  get_logger(   ),   ss.str(   ).c_str(   ) );
          }
          
          } // namespace nav2_system_tests

navigation2/nav2_system_tests/src/planning/planner_tester.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #ifndef PLANNING__PLANNER_TESTER_HPP_
          #define PLANNING__PLANNER_TESTER_HPP_
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <thread>
          #include <algorithm>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_msgs/action/compute_path_to_pose.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_msgs/srv/get_costmap.hpp"
          #include "nav2_msgs/srv/is_path_valid.hpp"
          #include "visualization_msgs/msg/marker.hpp"
          #include "nav2_util/costmap.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/transform_stamped.hpp"
          #include "tf2_msgs/msg/tf_message.hpp"
          #include "nav2_planner/planner_server.hpp"
          #include "tf2_ros/transform_broadcaster.h"
          
          namespace nav2_system_tests
          {
          
      43  class NavFnPlannerTester : public nav2_planner::PlannerServer
          {
          public:
      46   NavFnPlannerTester(   )
           : PlannerServer(   )
           {
           }
          
      51   void printCostmap(   )
           {
           // print costmap for debug
           for (  size_t i = 0; i != costmap_->getSizeInCellsX(   ) * costmap_->getSizeInCellsY(   ); i++ ) {
           if (  i % costmap_->getSizeInCellsX(   ) == 0 ) {
           std::cout << "" << std::endl;
           }
           std::cout << costmap_ros_->getCostmap(   )->getCharMap(   )[i] << " ";
           }
           std::cout << "" << std::endl;
           }
          
      63   void setCostmap(  nav2_util::Costmap * costmap )
           {
           nav2_msgs::msg::CostmapMetaData prop;
           nav2_msgs::msg::Costmap cm = costmap->get_costmap(  prop );
           prop = cm.metadata;
           costmap_ros_->getCostmap(   )->resizeMap(  
           prop.size_x,   prop.size_y,  
           prop.resolution,   prop.origin.position.x,   prop.origin.position.x );
           // Volatile prevents compiler from treating costmap_ptr as unused or changing its address
           volatile unsigned char * costmap_ptr = costmap_ros_->getCostmap(   )->getCharMap(   );
           delete[] costmap_ptr;
           costmap_ptr = new unsigned char[prop.size_x * prop.size_y];
           std::copy(  cm.data.begin(   ),   cm.data.end(   ),   costmap_ptr );
           }
          
      78   bool createPath(  
      79   const geometry_msgs::msg::PoseStamped & goal,  
      80   nav_msgs::msg::Path & path )
           {
           geometry_msgs::msg::PoseStamped start;
           if (  !nav2_util::getCurrentPose(  start,   *tf_,   "map",   "base_link",   0.1 ) ) {
           return false;
           }
           try {
           path = planners_["GridBased"]->createPlan(  start,   goal );
           // The situation when createPlan(   ) did not throw any exception
           // does not guarantee that plan was created correctly.
           // So it should be checked additionally that path is correct.
           if (  !path.poses.size(   ) ) {
           return false;
           }
           } catch (  ... ) {
           return false;
           }
           return true;
           }
          
     100   void onCleanup(  const rclcpp_lifecycle::State & state )
           {
           on_cleanup(  state );
           }
          
     105   void onActivate(  const rclcpp_lifecycle::State & state )
           {
           on_activate(  state );
           }
          
     110   void onDeactivate(  const rclcpp_lifecycle::State & state )
           {
           on_deactivate(  state );
           }
          
     115   void onConfigure(  const rclcpp_lifecycle::State & state )
           {
           on_configure(  state );
           }
          };
          
     121  enum class TaskStatus : int8_t
          {
           SUCCEEDED = 1,  
           FAILED = 2,  
           RUNNING = 3,  
          };
          
     128  class PlannerTester : public rclcpp::Node
          {
          public:
           using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
           using ComputePathToPoseResult = nav_msgs::msg::Path;
          
           PlannerTester(   );
           ~PlannerTester(   );
          
           // Activate the tester before running tests
           void activate(   );
           void deactivate(   );
          
           // Loads the provided map and and generates a costmap from it.
           void loadDefaultMap(   );
          
           // Alternatively,   use a preloaded 10x10 costmap
           void loadSimpleCostmap(  const nav2_util::TestCostmap & testCostmapType );
          
           // Runs a single test with default poses depending on the loaded map
           // Success criteria is a collision free path and a deviation to a
           // reference path smaller than a tolerance.
           bool defaultPlannerTest(  
           ComputePathToPoseResult & path,  
           const double deviation_tolerance = 1.0 );
          
          
           // Runs multiple tests with random initial and goal poses
           bool defaultPlannerRandomTests(  
           const unsigned int number_tests,  
           const float acceptable_fail_ratio );
          
           bool isPathValid(  nav_msgs::msg::Path & path );
          
          private:
           void setCostmap(   );
          
           TaskStatus createPlan(  
           const ComputePathToPoseCommand & goal,  
           ComputePathToPoseResult & path
            );
          
           bool is_active_;
           bool map_set_;
           bool costmap_set_;
           bool using_fake_costmap_;
          
           // Parameters of the costmap
           bool trinary_costmap_;
           bool track_unknown_space_;
           int lethal_threshold_;
           int unknown_cost_value_;
           nav2_util::TestCostmap testCostmapType_;
          
           // The static map
           std::shared_ptr<nav_msgs::msg::OccupancyGrid> map_;
          
           // The costmap representation of the static map
           std::unique_ptr<nav2_util::Costmap> costmap_;
          
           // The global planner
           std::shared_ptr<NavFnPlannerTester> planner_tester_;
          
           // The is path valid client
           rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr path_valid_client_;
          
           // A thread for spinning the ROS node
           std::unique_ptr<nav2_util::NodeThread> spin_thread_;
          
           // The tester must provide the robot pose through a transform
           std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
           std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
           rclcpp::TimerBase::SharedPtr transform_timer_;
           void publishRobotTransform(   );
           void startRobotTransform(   );
           void updateRobotPosition(  const geometry_msgs::msg::Point & position );
          
           // Occupancy grid publisher for visualization
           rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
           rclcpp::TimerBase::SharedPtr map_timer_;
           rclcpp::WallRate map_publish_rate_;
           void mapCallback(   );
          
           // Executes a test run with the provided end points.
           // Success criteria is a collision free path.
           // TODO(  orduno ): #443 Assuming a robot the size of a costmap cell
           bool plannerTest(  
           const geometry_msgs::msg::Point & robot_position,  
           const ComputePathToPoseCommand & goal,  
           ComputePathToPoseResult & path );
          
           bool isCollisionFree(  const ComputePathToPoseResult & path );
          
           bool isWithinTolerance(  
           const geometry_msgs::msg::Point & robot_position,  
           const ComputePathToPoseCommand & goal,  
           const ComputePathToPoseResult & path ) const;
          
           bool isWithinTolerance(  
           const geometry_msgs::msg::Point & robot_position,  
           const ComputePathToPoseCommand & goal,  
           const ComputePathToPoseResult & path,  
           const double deviationTolerance,  
           const ComputePathToPoseResult & reference_path ) const;
          
           void printPath(  const ComputePathToPoseResult & path ) const;
          };
          
          } // namespace nav2_system_tests
          
          #endif // PLANNING__PLANNER_TESTER_HPP_

navigation2/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <vector>
          
          #include "rclcpp/rclcpp.hpp"
          #include "planner_tester.hpp"
          #include "nav2_util/lifecycle_utils.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::PlannerTester;
          using nav2_util::TestCostmap;
          
          using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
          using ComputePathToPoseResult = nav_msgs::msg::Path;
          
      31  TEST(  testSimpleCostmaps,   testSimpleCostmaps )
          {
           auto obj = std::make_shared<PlannerTester>(   );
          
           std::vector<TestCostmap> costmaps = {
           TestCostmap::open_space,  
           TestCostmap::bounded,  
           TestCostmap::top_left_obstacle,  
           TestCostmap::bottom_left_obstacle,  
           TestCostmap::maze1,  
           TestCostmap::maze2
           };
          
           ComputePathToPoseResult result;
          
           obj->activate(   );
          
           for (  auto costmap : costmaps ) {
           obj->loadSimpleCostmap(  costmap );
           EXPECT_EQ(  true,   obj->defaultPlannerTest(  result ) );
           }
          }
          
      54  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp

       1  // Copyright (  c ) 2022 Joshua Wallace
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <vector>
          
          #include "nav2_msgs/srv/is_path_valid.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "planner_tester.hpp"
          #include "nav2_util/lifecycle_utils.hpp"
          
          using nav2_system_tests::PlannerTester;
          using nav2_util::TestCostmap;
          
      27  TEST(  testIsPathValid,   testIsPathValid )
          {
           auto planner_tester = std::make_shared<PlannerTester>(   );
           planner_tester->activate(   );
           planner_tester->loadSimpleCostmap(  TestCostmap::top_left_obstacle );
          
           nav_msgs::msg::Path path;
          
           // empty path
           bool is_path_valid = planner_tester->isPathValid(  path );
           EXPECT_FALSE(  is_path_valid );
          
           // invalid path
           for (  float i = 0; i < 10; i += 1.0 ) {
           for (  float j = 0; j < 10; j += 1.0 ) {
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = i;
           pose.pose.position.y = j;
           path.poses.push_back(  pose );
           }
           }
           is_path_valid = planner_tester->isPathValid(  path );
           EXPECT_FALSE(  is_path_valid );
          
           // valid path
           path.poses.clear(   );
           for (  float i = 0; i < 10; i += 1.0 ) {
           geometry_msgs::msg::PoseStamped pose;
           pose.pose.position.x = 1.0;
           pose.pose.position.y = i;
           path.poses.push_back(  pose );
           }
           is_path_valid = planner_tester->isPathValid(  path );
           EXPECT_TRUE(  is_path_valid );
          }
          
      63  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
           return all_successful;
          }

navigation2/nav2_system_tests/src/planning/test_planner_plugins.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <vector>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "planner_tester.hpp"
          #include "nav2_util/lifecycle_utils.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::PlannerTester;
          using nav2_util::TestCostmap;
          
          using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
          using ComputePathToPoseResult = nav_msgs::msg::Path;
          
      33  void callback(  const nav_msgs::msg::Path::ConstSharedPtr /*grid*/ )
          {
          }
          
      37  void testSmallPathValidityAndOrientation(  std::string plugin,   double length )
          {
           auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>(   );
           rclcpp_lifecycle::State state;
           obj->set_parameter(  rclcpp::Parameter(  "GridBased.plugin",   plugin ) );
           obj->declare_parameter(  
           "GridBased.use_final_approach_orientation",   rclcpp::ParameterValue(  false ) );
           obj->onConfigure(  state );
          
           geometry_msgs::msg::PoseStamped start;
           geometry_msgs::msg::PoseStamped goal;
          
           start.pose.position.x = 0.5;
           start.pose.position.y = 0.5;
           start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  M_PI_2 );
           start.header.frame_id = "map";
          
           goal.pose.position.x = 0.5;
           goal.pose.position.y = start.pose.position.y + length;
           goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  -M_PI );
           goal.header.frame_id = "map";
          
           // Test without use_final_approach_orientation
           // expecting end path pose orientation to be equal to goal orientation
           auto path = obj->getPlan(  start,   goal,   "GridBased" );
           EXPECT_GT(  (  int )path.poses.size(   ),   0 );
           EXPECT_NEAR(  tf2::getYaw(  path.poses.back(   ).pose.orientation ),   -M_PI,   0.01 );
           // obj->onCleanup(  state );
           obj.reset(   );
          }
          
      68  void testSmallPathValidityAndNoOrientation(  std::string plugin,   double length )
          {
           auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>(   );
           rclcpp_lifecycle::State state;
           obj->set_parameter(  rclcpp::Parameter(  "GridBased.plugin",   plugin ) );
          
           // Test WITH use_final_approach_orientation
           // expecting end path pose orientation to be equal to approach orientation
           // which in the one pose corner case should be the start pose orientation
           obj->declare_parameter(  
           "GridBased.use_final_approach_orientation",   rclcpp::ParameterValue(  true ) );
           obj->set_parameter(  rclcpp::Parameter(  "GridBased.use_final_approach_orientation",   true ) );
           obj->onConfigure(  state );
          
           geometry_msgs::msg::PoseStamped start;
           geometry_msgs::msg::PoseStamped goal;
          
           start.pose.position.x = 0.5;
           start.pose.position.y = 0.5;
           start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  M_PI_2 );
           start.header.frame_id = "map";
          
           goal.pose.position.x = 0.5;
           goal.pose.position.y = start.pose.position.y + length;
           goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(  -M_PI );
           goal.header.frame_id = "map";
          
           auto path = obj->getPlan(  start,   goal,   "GridBased" );
           EXPECT_GT(  (  int )path.poses.size(   ),   0 );
          
           int path_size = path.poses.size(   );
           if (  path_size == 1 ) {
           EXPECT_NEAR(  
           tf2::getYaw(  path.poses.back(   ).pose.orientation ),  
           tf2::getYaw(  start.pose.orientation ),  
           0.01 );
           } else {
           double dx = path.poses.back(   ).pose.position.x - path.poses.front(   ).pose.position.x;
           double dy = path.poses.back(   ).pose.position.y - path.poses.front(   ).pose.position.y;
           EXPECT_NEAR(  
           tf2::getYaw(  path.poses.back(   ).pose.orientation ),  
           atan2(  dy,   dx ),  
           0.01 );
           }
           // obj->onCleanup(  state );
           obj.reset(   );
          }
          
     116  TEST(  testPluginMap,   Failures )
          {
           auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>(   );
           rclcpp_lifecycle::State state;
           obj->set_parameter(  rclcpp::Parameter(  "expected_planner_frequency",   100000.0 ) );
           obj->onConfigure(  state );
           obj->create_subscription<nav_msgs::msg::Path>(  
           "plan",   rclcpp::SystemDefaultsQoS(   ),   callback );
          
           geometry_msgs::msg::PoseStamped start;
           geometry_msgs::msg::PoseStamped goal;
           std::string plugin_fake = "fake";
           std::string plugin_none = "";
           auto path = obj->getPlan(  start,   goal,   plugin_none );
           EXPECT_EQ(  path.header.frame_id,   std::string(  "map" ) );
          
           path = obj->getPlan(  start,   goal,   plugin_fake );
           EXPECT_EQ(  path.poses.size(   ),   0ul );
          
           obj->onCleanup(  state );
          }
          
     138  TEST(  testPluginMap,   Smac2dEqualStartGoal )
          {
           testSmallPathValidityAndOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.0 );
          }
          
     143  TEST(  testPluginMap,   Smac2dEqualStartGoalN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.0 );
          }
          
     148  TEST(  testPluginMap,   Smac2dVerySmallPath )
          {
           testSmallPathValidityAndOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.00001 );
          }
          
     153  TEST(  testPluginMap,   Smac2dVerySmallPathN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.00001 );
          }
          
     158  TEST(  testPluginMap,   Smac2dBelowCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.09 );
          }
          
     163  TEST(  testPluginMap,   Smac2dBelowCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.09 );
          }
          
     168  TEST(  testPluginMap,   Smac2dJustAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.102 );
          }
          
     173  TEST(  testPluginMap,   Smac2dJustAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_smac_planner/SmacPlanner2D",   0.102 );
          }
          
     178  TEST(  testPluginMap,   Smac2dAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_smac_planner/SmacPlanner2D",   1.5 );
          }
          
     183  TEST(  testPluginMap,   Smac2dAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_smac_planner/SmacPlanner2D",   1.5 );
          }
          
     188  TEST(  testPluginMap,   NavFnEqualStartGoal )
          {
           testSmallPathValidityAndOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.0 );
          }
          
     193  TEST(  testPluginMap,   NavFnEqualStartGoalN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.0 );
          }
          
     198  TEST(  testPluginMap,   NavFnVerySmallPath )
          {
           testSmallPathValidityAndOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.00001 );
          }
          
     203  TEST(  testPluginMap,   NavFnVerySmallPathN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.00001 );
          }
          
     208  TEST(  testPluginMap,   NavFnBelowCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.09 );
          }
          
     213  TEST(  testPluginMap,   NavFnBelowCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.09 );
          }
          
     218  TEST(  testPluginMap,   NavFnJustAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.102 );
          }
          
     223  TEST(  testPluginMap,   NavFnJustAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_navfn_planner/NavfnPlanner",   0.102 );
          }
          
     228  TEST(  testPluginMap,   NavFnAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_navfn_planner/NavfnPlanner",   1.5 );
          }
          
     233  TEST(  testPluginMap,   NavFnAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_navfn_planner/NavfnPlanner",   1.5 );
          }
          
     238  TEST(  testPluginMap,   ThetaStarEqualStartGoal )
          {
           testSmallPathValidityAndOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.0 );
          }
          
     243  TEST(  testPluginMap,   ThetaStarEqualStartGoalN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.0 );
          }
          
     248  TEST(  testPluginMap,   ThetaStarVerySmallPath )
          {
           testSmallPathValidityAndOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.00001 );
          }
          
     253  TEST(  testPluginMap,   ThetaStarVerySmallPathN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.00001 );
          }
          
     258  TEST(  testPluginMap,   ThetaStarBelowCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.09 );
          }
          
     263  TEST(  testPluginMap,   ThetaStarBelowCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.09 );
          }
          
     268  TEST(  testPluginMap,   ThetaStarJustAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.102 );
          }
          
     273  TEST(  testPluginMap,   ThetaStarJustAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   0.102 );
          }
          
     278  TEST(  testPluginMap,   ThetaStarAboveCostmapResolution )
          {
           testSmallPathValidityAndOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   1.5 );
          }
          
     283  TEST(  testPluginMap,   ThetaStarAboveCostmapResolutionN )
          {
           testSmallPathValidityAndNoOrientation(  "nav2_theta_star_planner/ThetaStarPlanner",   1.5 );
          }
          
     288  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/planning/test_planner_random_node.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <vector>
          #include <iostream>
          
          #include "rclcpp/rclcpp.hpp"
          #include "planner_tester.hpp"
          
          using namespace std::chrono_literals;
          
          using nav2_system_tests::PlannerTester;
          using nav2_util::TestCostmap;
          
          using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
          using ComputePathToPoseResult = nav_msgs::msg::Path;
          
      31  TEST(  testWithHundredRandomEndPoints,   testWithHundredRandomEndPoints )
          {
           auto obj = std::make_shared<PlannerTester>(   );
           obj->activate(   );
           obj->loadDefaultMap(   );
          
           bool success = false;
           int num_tries = 3;
           for (  int i = 0; i != num_tries; i++ ) {
           success = success || obj->defaultPlannerRandomTests(  100,   0.1 );
           if (  success ) {
           break;
           }
           }
          
           EXPECT_EQ(  true,   success );
          }
          
      49  int main(  int argc,   char ** argv )
          {
           ::testing::InitGoogleTest(  &argc,   argv );
          
           // initialize ROS
           rclcpp::init(  argc,   argv );
          
           bool all_successful = RUN_ALL_TESTS(   );
          
           // shutdown ROS
           rclcpp::shutdown(   );
          
           return all_successful;
          }

navigation2/nav2_system_tests/src/updown/test_updown.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <random>
          #include <string>
          #include <vector>
          #include <memory>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
          #include "rcutils/cmdline_parser.h"
          
          using namespace std::chrono_literals;
          
      26  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           RCLCPP_INFO(  rclcpp::get_logger(  "test_updown" ),   "Initializing test" );
           auto node = std::make_shared<rclcpp::Node>(  "lifecycle_manager_service_client" );
           nav2_lifecycle_manager::LifecycleManagerClient client_nav(  "lifecycle_manager_navigation",   node );
           nav2_lifecycle_manager::LifecycleManagerClient client_loc(  "lifecycle_manager_localization",   node );
           bool test_passed = true;
          
           // Wait for a few seconds to let all of the nodes come up
           std::this_thread::sleep_for(  5s );
          
           // Start the nav2 system,   bringing it to the ACTIVE state
           client_nav.startup(   );
           client_loc.startup(   );
          
           // Wait for a couple secs to make sure the nodes have processed all discovery
           // info before starting
           RCLCPP_INFO(  rclcpp::get_logger(  "test_updown" ),   "Waiting for nodes to be active" );
           std::this_thread::sleep_for(  2s );
          
           // The system should now be active
           int retries = 0;
           while (  (  client_nav.is_active(   ) != nav2_lifecycle_manager::SystemStatus::ACTIVE ) &&
           (  client_loc.is_active(   ) != nav2_lifecycle_manager::SystemStatus::ACTIVE ) &&
           (  retries < 10 ) )
           {
           std::this_thread::sleep_for(  2s );
           retries++;
           }
           if (  retries == 10 ) {
           // the system isn't active
           RCLCPP_ERROR(  rclcpp::get_logger(  "test_updown" ),   "System startup failed" );
           test_passed = false;
           }
          
           // Shut down the nav2 system,   bringing it to the FINALIZED state
           client_nav.shutdown(   );
           client_loc.shutdown(   );
          
           if (  test_passed ) {
           RCLCPP_INFO(  
           rclcpp::get_logger(  "test_updown" ),  
           "**************************************************** TEST PASSED!" );
           } else {
           RCLCPP_INFO(  
           rclcpp::get_logger(  "test_updown" ),  
           "**************************************************** TEST FAILED!" );
           }
           rclcpp::shutdown(   );
           return 0;
          }

navigation2/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp

          // Copyright 2020 Anshumaan Singh
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
          #define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
          
          #include <cmath>
          #include <chrono>
          #include <vector>
          #include <queue>
          #include <algorithm>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          
          const double INF_COST = DBL_MAX;
          const int LETHAL_COST = 252;
          
          struct coordsM
          {
           int x,   y;
          };
          
          struct coordsW
          {
           double x,   y;
          };
          
          struct tree_node
          {
           int x,   y;
           double g = INF_COST;
           double h = INF_COST;
           const tree_node * parent_id = nullptr;
           bool is_in_queue = false;
           double f = INF_COST;
          };
          
          struct comp
          {
           bool operator(   )(  const tree_node * p1,   const tree_node * p2 )
           {
           return (  p1->f ) > (  p2->f );
           }
          };
          
          namespace theta_star
          {
      59  class ThetaStar
          {
          public:
           coordsM src_{},   dst_{};
           nav2_costmap_2d::Costmap2D * costmap_{};
          
           /// weight on the costmap traversal cost
           double w_traversal_cost_;
           /// weight on the euclidean distance cost (  used for calculations of g_cost )
           double w_euc_cost_;
           /// weight on the heuristic cost (  used for h_cost calculations )
           double w_heuristic_cost_;
           /// parameter to set the number of adjacent nodes to be searched on
           int how_many_corners_;
           /// the x-directional and y-directional lengths of the map respectively
           int size_x_,   size_y_;
          
      76   ThetaStar(   );
          
      78   ~ThetaStar(   ) = default;
          
           /**
           * @brief it iteratively searches upon the nodes in the queue (  open list ) until the
           * current node is the goal pose or the size of queue becomes 0
           * @param raw_path is used to return the path obtained by executing the algorithm
           * @return true if a path is found,   false if no path is found in between the start and goal pose
           */
      86   bool generatePath(  std::vector<coordsW> & raw_path );
          
           /**
           * @brief this function checks whether the cost of a point(  cx,   cy ) on the costmap is less than the LETHAL_COST
           * @return the result of the check
           */
      92   inline bool isSafe(  const int & cx,   const int & cy ) const
           {
           return costmap_->getCost(  cx,   cy ) < LETHAL_COST;
           }
          
           /**
           * @brief initialises the values of the start and goal points
           */
     100   void setStartAndGoal(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal );
          
           /**
           * @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST
           * @return true if the cost of any one of the points is greater than LETHAL_COST
           */
     108   bool isUnsafeToPlan(   ) const
           {
           return !(  isSafe(  src_.x,   src_.y ) ) || !(  isSafe(  dst_.x,   dst_.y ) );
           }
          
           int nodes_opened = 0;
          
          protected:
           /// for the coordinates (  x,  y ),   it stores at node_position_[size_x_ * y + x],  
           /// the pointer to the location at which the data of the node is present in nodes_data_
           /// it is initialised with size_x_ * size_y_ elements
           /// and its number of elements increases to account for a change in map size
           std::vector<tree_node *> node_position_;
          
           /// the vector nodes_data_ stores the coordinates,   costs and index of the parent node,  
           /// and whether or not the node is present in queue_,   for all the nodes searched
           /// it is initialised with no elements
           /// and its size increases depending on the number of nodes searched
           std::vector<tree_node> nodes_data_;
          
           /// this is the priority queue (  open_list ) to select the next node to be expanded
           std::priority_queue<tree_node *,   std::vector<tree_node *>,   comp> queue_;
          
           /// it is a counter like variable used to generate consecutive indices
           /// such that the data for all the nodes (  in open and closed lists ) could be stored
           /// consecutively in nodes_data_
           int index_generated_;
          
           const coordsM moves[8] = {{0,   1},  
           {0,   -1},  
           {1,   0},  
           {-1,   0},  
           {1,   -1},  
           {-1,   1},  
           {1,   1},  
           {-1,   -1}};
          
           tree_node * exp_node;
          
          
           /** @brief it performs a line of sight (  los ) check between the current node and the parent node of its parent node;
           * if an los is found and the new costs calculated are lesser,   then the cost and parent node
           * of the current node is updated
           * @param data of the current node
           */
           void resetParent(  tree_node * curr_data );
          
           /**
           * @brief this function expands the current node
           * @param curr_data used to send the data of the current node
           * @param curr_id used to send the index of the current node as stored in nodes_position_
           */
           void setNeighbors(  const tree_node * curr_data );
          
           /**
           * @brief performs the line of sight check using Bresenham's Algorithm,  
           * and has been modified to calculate the traversal cost incurred in a straight line path between
           * the two points whose coordinates are (  x0,   y0 ) and (  x1,   y1 )
           * @param sl_cost is used to return the cost thus incurred
           * @return true if a line of sight exists between the points
           */
           bool losCheck(  
           const int & x0,   const int & y0,   const int & x1,   const int & y1,  
           double & sl_cost ) const;
          
           /**
           * @brief it returns the path by backtracking from the goal to the start,   by using their parent nodes
           * @param raw_points used to return the path thus found
           * @param curr_id sends in the index of the goal coordinate,   as stored in nodes_position
           */
           void backtrace(  std::vector<coordsW> & raw_points,   const tree_node * curr_n ) const;
          
           /**
           * @brief it is an overloaded function to ease the cost calculations while performing the LOS check
           * @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (  cx,   cy ) at every instance; it is also being returned
           * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise
           */
           bool isSafe(  const int & cx,   const int & cy,   double & cost ) const
           {
           double curr_cost = getCost(  cx,   cy );
           if (  curr_cost < LETHAL_COST ) {
           cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
           return true;
           } else {
           return false;
           }
           }
          
           /*
           * @brief this function scales the costmap cost by shifting the origin to 25 and then multiply
           * the actual costmap cost by 0.9 to keep the output in the range of [25,   255 )
           */
           inline double getCost(  const int & cx,   const int & cy ) const
           {
           return 26 + 0.9 * costmap_->getCost(  cx,   cy );
           }
          
           /**
           * @brief for the point(  cx,   cy ),   its traversal cost is calculated by
           * <parameter>*(  <actual_traversal_cost_from_costmap> )^2/(  <max_cost> )^2
           * @return the traversal cost thus calculated
           */
           inline double getTraversalCost(  const int & cx,   const int & cy )
           {
           double curr_cost = getCost(  cx,   cy );
           return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
           }
          
           /**
           * @brief calculates the piecewise straight line euclidean distances by
           * <euc_cost_parameter>*<euclidean distance between the points (  ax,   ay ) and (  bx,   by )>
           * @return the distance thus calculated
           */
           inline double getEuclideanCost(  const int & ax,   const int & ay,   const int & bx,   const int & by )
           {
           return w_euc_cost_ * std::hypot(  ax - bx,   ay - by );
           }
          
           /**
           * @brief for the point(  cx,   cy ),   its heuristic cost is calculated by
           * <heuristic_cost_parameter>*<euclidean distance between the point and goal>
           * @return the heuristic cost
           */
           inline double getHCost(  const int & cx,   const int & cy )
           {
           return w_heuristic_cost_ * std::hypot(  cx - dst_.x,   cy - dst_.y );
           }
          
           /**
           * @brief checks if the given coordinates(  cx,   cy ) lies within the map
           * @return the result of the check
           */
           inline bool withinLimits(  const int & cx,   const int & cy ) const
           {
           return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_;
           }
          
           /**
           * @brief checks if the coordinates of a node is the goal or not
           * @return the result of the check
           */
           inline bool isGoal(  const tree_node & this_node ) const
           {
           return this_node.x == dst_.x && this_node.y == dst_.y;
           }
          
           /**
           * @brief initialises the node_position_ vector by storing -1 as index for all points(  x,   y ) within the limits of the map
           * @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases
           */
           void initializePosn(  int size_inc = 0 );
          
           /**
           * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ]
           * @param id_this a pointer to the location at which the data of the point(  cx,   cy ) is stored in nodes_data_
           */
           inline void addIndex(  const int & cx,   const int & cy,   tree_node * node_this )
           {
           node_position_[size_x_ * cy + cx] = node_this;
           }
          
           /**
           * @brief retrieves the pointer of the location at which the data of the point(  cx,   cy ) is stored in nodes_data
           * @return id_this is the pointer to that location
           */
           inline tree_node * getIndex(  const int & cx,   const int & cy )
           {
           return node_position_[size_x_ * cy + cx];
           }
          
           /**
           * @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(  x,   y )
           * @param id_this is the index at which the data is stored/has to be stored for that node
           */
           void addToNodesData(  const int & id_this )
           {
           if (  static_cast<int>(  nodes_data_.size(   ) ) <= id_this ) {
           nodes_data_.push_back(  {} );
           } else {
           nodes_data_[id_this] = {};
           }
           }
          
           /**
           * @brief initialises the values of global variables at beginning of the execution of the generatePath function
           */
           void resetContainers(   );
          
           /**
           * @brief clears the priority queue after each execution of the generatePath function
           */
           void clearQueue(   )
           {
           queue_ = std::priority_queue<tree_node *,   std::vector<tree_node *>,   comp>(   );
           }
          };
          } // namespace theta_star
          
          #endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_

navigation2/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp

          // Copyright 2020 Anshumaan Singh
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
          #define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
          
          #include <iostream>
          #include <cmath>
          #include <string>
          #include <chrono>
          #include <queue>
          #include <algorithm>
          #include <memory>
          #include <vector>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_core/global_planner.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_costmap_2d/costmap_2d_ros.hpp"
          #include "nav2_costmap_2d/cost_values.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_theta_star_planner/theta_star.hpp"
          #include "nav2_util/geometry_utils.hpp"
          
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_theta_star_planner
          {
          
      42  class ThetaStarPlanner : public nav2_core::GlobalPlanner
          {
          public:
           void configure(  
           const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
           std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
          
           void cleanup(   ) override;
          
           void activate(   ) override;
          
           void deactivate(   ) override;
          
           nav_msgs::msg::Path createPlan(  
           const geometry_msgs::msg::PoseStamped & start,  
           const geometry_msgs::msg::PoseStamped & goal ) override;
          
          protected:
           std::shared_ptr<tf2_ros::Buffer> tf_;
           rclcpp::Clock::SharedPtr clock_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "ThetaStarPlanner" )};
           std::string global_frame_,   name_;
           bool use_final_approach_orientation_;
          
           // parent node weak ptr
           rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node_;
          
           std::unique_ptr<theta_star::ThetaStar> planner_;
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          
          
           /**
           * @brief the function responsible for calling the algorithm and retrieving a path from it
           * @return global_path is the planned path to be taken
           */
      80   void getPlan(  nav_msgs::msg::Path & global_path );
          
           /**
           * @brief interpolates points between the consecutive waypoints of the path
           * @param raw_path is used to send in the path received from the planner
           * @param dist_bw_points is used to send in the interpolation_resolution (  which has been set as the costmap resolution )
           * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other
           */
      88   static nav_msgs::msg::Path linearInterpolation(  
           const std::vector<coordsW> & raw_path,  
           const double & dist_bw_points );
          
           /**
           * @brief Callback executed when a paramter change is detected
           * @param parameters list of changed parameters
           */
           rcl_interfaces::msg::SetParametersResult
      97   dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          };
          } // namespace nav2_theta_star_planner
          
          #endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_

navigation2/nav2_theta_star_planner/src/theta_star.cpp

       1  // Copyright 2020 Anshumaan Singh
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include "nav2_theta_star_planner/theta_star.hpp"
          
          namespace theta_star
          {
          
      21  ThetaStar::ThetaStar(   )
          : w_traversal_cost_(  1.0 ),  
           w_euc_cost_(  2.0 ),  
           w_heuristic_cost_(  1.0 ),  
           how_many_corners_(  8 ),  
           size_x_(  0 ),  
           size_y_(  0 ),  
           index_generated_(  0 )
          {
           exp_node = new tree_node;
          }
          
      33  void ThetaStar::setStartAndGoal(  
      34   const geometry_msgs::msg::PoseStamped & start,  
      35   const geometry_msgs::msg::PoseStamped & goal )
          {
           unsigned int s[2],   d[2];
           costmap_->worldToMap(  start.pose.position.x,   start.pose.position.y,   s[0],   s[1] );
           costmap_->worldToMap(  goal.pose.position.x,   goal.pose.position.y,   d[0],   d[1] );
          
           src_ = {static_cast<int>(  s[0] ),   static_cast<int>(  s[1] )};
           dst_ = {static_cast<int>(  d[0] ),   static_cast<int>(  d[1] )};
          }
          
      45  bool ThetaStar::generatePath(  std::vector<coordsW> & raw_path )
          {
           resetContainers(   );
           addToNodesData(  index_generated_ );
           double src_g_cost = getTraversalCost(  src_.x,   src_.y ),   src_h_cost = getHCost(  src_.x,   src_.y );
           nodes_data_[index_generated_] =
           {src_.x,   src_.y,   src_g_cost,   src_h_cost,   &nodes_data_[index_generated_],   true,  
           src_g_cost + src_h_cost};
           queue_.push(  {&nodes_data_[index_generated_]} );
           addIndex(  src_.x,   src_.y,   &nodes_data_[index_generated_] );
           tree_node * curr_data = &nodes_data_[index_generated_];
           index_generated_++;
           nodes_opened = 0;
          
           while (  !queue_.empty(   ) ) {
           nodes_opened++;
          
           if (  isGoal(  *curr_data ) ) {
           break;
           }
          
           resetParent(  curr_data );
           setNeighbors(  curr_data );
          
           curr_data = queue_.top(   );
           queue_.pop(   );
           }
          
           if (  queue_.empty(   ) ) {
           raw_path.clear(   );
           return false;
           }
          
           backtrace(  raw_path,   curr_data );
           clearQueue(   );
          
           return true;
          }
          
      84  void ThetaStar::resetParent(  tree_node * curr_data )
          {
           double g_cost,   los_cost = 0;
           curr_data->is_in_queue = false;
           const tree_node * curr_par = curr_data->parent_id;
           const tree_node * maybe_par = curr_par->parent_id;
          
           if (  losCheck(  curr_data->x,   curr_data->y,   maybe_par->x,   maybe_par->y,   los_cost ) ) {
           g_cost = maybe_par->g +
           getEuclideanCost(  curr_data->x,   curr_data->y,   maybe_par->x,   maybe_par->y ) + los_cost;
          
           if (  g_cost < curr_data->g ) {
           curr_data->parent_id = maybe_par;
           curr_data->g = g_cost;
           curr_data->f = g_cost + curr_data->h;
           }
           }
          }
          
     103  void ThetaStar::setNeighbors(  const tree_node * curr_data )
          {
           int mx,   my;
           tree_node * m_id = nullptr;
           double g_cost,   h_cost,   cal_cost;
          
           for (  int i = 0; i < how_many_corners_; i++ ) {
           mx = curr_data->x + moves[i].x;
           my = curr_data->y + moves[i].y;
          
           if (  withinLimits(  mx,   my ) ) {
           if (  !isSafe(  mx,   my ) ) {
           continue;
           }
           } else {
           continue;
           }
          
           g_cost = curr_data->g + getEuclideanCost(  curr_data->x,   curr_data->y,   mx,   my ) +
           getTraversalCost(  mx,   my );
          
           m_id = getIndex(  mx,   my );
          
           if (  m_id == nullptr ) {
           addToNodesData(  index_generated_ );
           m_id = &nodes_data_[index_generated_];
           addIndex(  mx,   my,   m_id );
           index_generated_++;
           }
          
           exp_node = m_id;
          
           h_cost = getHCost(  mx,   my );
           cal_cost = g_cost + h_cost;
           if (  exp_node->f > cal_cost ) {
           exp_node->g = g_cost;
           exp_node->h = h_cost;
           exp_node->f = cal_cost;
           exp_node->parent_id = curr_data;
           if (  !exp_node->is_in_queue ) {
           exp_node->x = mx;
           exp_node->y = my;
           exp_node->is_in_queue = true;
           queue_.push(  {m_id} );
           }
           }
           }
          }
          
     152  void ThetaStar::backtrace(  std::vector<coordsW> & raw_points,   const tree_node * curr_n ) const
          {
           std::vector<coordsW> path_rev;
           coordsW world{};
           do {
           costmap_->mapToWorld(  curr_n->x,   curr_n->y,   world.x,   world.y );
           path_rev.push_back(  world );
           if (  path_rev.size(   ) > 1 ) {
           curr_n = curr_n->parent_id;
           }
           } while (  curr_n->parent_id != curr_n );
           costmap_->mapToWorld(  curr_n->x,   curr_n->y,   world.x,   world.y );
           path_rev.push_back(  world );
          
           raw_points.reserve(  path_rev.size(   ) );
           for (  int i = static_cast<int>(  path_rev.size(   ) ) - 1; i >= 0; i-- ) {
           raw_points.push_back(  path_rev[i] );
           }
          }
          
     172  bool ThetaStar::losCheck(  
           const int & x0,   const int & y0,   const int & x1,   const int & y1,  
           double & sl_cost ) const
          {
           sl_cost = 0;
          
           int cx,   cy;
           int dy = abs(  y1 - y0 ),   dx = abs(  x1 - x0 ),   f = 0;
           int sx,   sy;
           sx = x1 > x0 ? 1 : -1;
           sy = y1 > y0 ? 1 : -1;
          
           int u_x = (  sx - 1 ) / 2;
           int u_y = (  sy - 1 ) / 2;
           cx = x0;
           cy = y0;
          
           if (  dx >= dy ) {
           while (  cx != x1 ) {
           f += dy;
           if (  f >= dx ) {
           if (  !isSafe(  cx + u_x,   cy + u_y,   sl_cost ) ) {
           return false;
           }
           cy += sy;
           f -= dx;
           }
           if (  f != 0 && !isSafe(  cx + u_x,   cy + u_y,   sl_cost ) ) {
           return false;
           }
           if (  dy == 0 && !isSafe(  cx + u_x,   cy,   sl_cost ) && !isSafe(  cx + u_x,   cy - 1,   sl_cost ) ) {
           return false;
           }
           cx += sx;
           }
           } else {
           while (  cy != y1 ) {
           f = f + dx;
           if (  f >= dy ) {
           if (  !isSafe(  cx + u_x,   cy + u_y,   sl_cost ) ) {
           return false;
           }
           cx += sx;
           f -= dy;
           }
           if (  f != 0 && !isSafe(  cx + u_x,   cy + u_y,   sl_cost ) ) {
           return false;
           }
           if (  dx == 0 && !isSafe(  cx,   cy + u_y,   sl_cost ) && !isSafe(  cx - 1,   cy + u_y,   sl_cost ) ) {
           return false;
           }
           cy += sy;
           }
           }
           return true;
          }
          
     229  void ThetaStar::resetContainers(   )
          {
           index_generated_ = 0;
           int last_size_x = size_x_;
           int last_size_y = size_y_;
           int curr_size_x = static_cast<int>(  costmap_->getSizeInCellsX(   ) );
           int curr_size_y = static_cast<int>(  costmap_->getSizeInCellsY(   ) );
           if (  (  (  last_size_x != curr_size_x ) || (  last_size_y != curr_size_y ) ) &&
           static_cast<int>(  node_position_.size(   ) ) < (  curr_size_x * curr_size_y ) )
           {
           initializePosn(  curr_size_y * curr_size_x - last_size_y * last_size_x );
           nodes_data_.reserve(  curr_size_x * curr_size_y );
           } else {
           initializePosn(   );
           }
           size_x_ = curr_size_x;
           size_y_ = curr_size_y;
          }
          
     248  void ThetaStar::initializePosn(  int size_inc )
          {
           int i = 0;
          
           if (  !node_position_.empty(   ) ) {
           for (  ; i < size_x_ * size_y_; i++ ) {
           node_position_[i] = nullptr;
           }
           }
          
           for (  ; i < size_inc; i++ ) {
           node_position_.push_back(  nullptr );
           }
          }
          } // namespace theta_star

navigation2/nav2_theta_star_planner/src/theta_star_planner.cpp

       1  // Copyright 2020 Anshumaan Singh
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <memory>
          #include <string>
          #include "nav2_theta_star_planner/theta_star_planner.hpp"
          #include "nav2_theta_star_planner/theta_star.hpp"
          
          namespace nav2_theta_star_planner
          {
      23  void ThetaStarPlanner::configure(  
      24   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      25   std::string name,   std::shared_ptr<tf2_ros::Buffer> tf,  
      26   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
          {
           planner_ = std::make_unique<theta_star::ThetaStar>(   );
           parent_node_ = parent;
           auto node = parent_node_.lock(   );
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
           name_ = name;
           tf_ = tf;
           planner_->costmap_ = costmap_ros->getCostmap(   );
           global_frame_ = costmap_ros->getGlobalFrameID(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name_ + ".how_many_corners",   rclcpp::ParameterValue(  8 ) );
          
           node->get_parameter(  name_ + ".how_many_corners",   planner_->how_many_corners_ );
          
           if (  planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4 ) {
           planner_->how_many_corners_ = 8;
           RCLCPP_WARN(  logger_,   "Your value for - .how_many_corners was overridden,   and is now set to 8" );
           }
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name_ + ".w_euc_cost",   rclcpp::ParameterValue(  1.0 ) );
           node->get_parameter(  name_ + ".w_euc_cost",   planner_->w_euc_cost_ );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name_ + ".w_traversal_cost",   rclcpp::ParameterValue(  2.0 ) );
           node->get_parameter(  name_ + ".w_traversal_cost",   planner_->w_traversal_cost_ );
          
           planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0;
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   name + ".use_final_approach_orientation",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  name + ".use_final_approach_orientation",   use_final_approach_orientation_ );
          }
          
      63  void ThetaStarPlanner::cleanup(   )
          {
           RCLCPP_INFO(  logger_,   "CleaningUp plugin %s of type nav2_theta_star_planner",   name_.c_str(   ) );
           planner_.reset(   );
          }
          
      69  void ThetaStarPlanner::activate(   )
          {
           RCLCPP_INFO(  logger_,   "Activating plugin %s of type nav2_theta_star_planner",   name_.c_str(   ) );
           // Add callback for dynamic parameters
           auto node = parent_node_.lock(   );
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &ThetaStarPlanner::dynamicParametersCallback,   this,   std::placeholders::_1 ) );
          }
          
      78  void ThetaStarPlanner::deactivate(   )
          {
           RCLCPP_INFO(  logger_,   "Deactivating plugin %s of type nav2_theta_star_planner",   name_.c_str(   ) );
          }
          
      83  nav_msgs::msg::Path ThetaStarPlanner::createPlan(  
      84   const geometry_msgs::msg::PoseStamped & start,  
      85   const geometry_msgs::msg::PoseStamped & goal )
          {
           nav_msgs::msg::Path global_path;
           auto start_time = std::chrono::steady_clock::now(   );
          
           // Corner case of start and goal beeing on the same cell
           unsigned int mx_start,   my_start,   mx_goal,   my_goal;
           planner_->costmap_->worldToMap(  start.pose.position.x,   start.pose.position.y,   mx_start,   my_start );
           planner_->costmap_->worldToMap(  goal.pose.position.x,   goal.pose.position.y,   mx_goal,   my_goal );
           if (  mx_start == mx_goal && my_start == my_goal ) {
           if (  planner_->costmap_->getCost(  mx_start,   my_start ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
           RCLCPP_WARN(  logger_,   "Failed to create a unique pose path because of obstacles" );
           return global_path;
           }
           global_path.header.stamp = clock_->now(   );
           global_path.header.frame_id = global_frame_;
           geometry_msgs::msg::PoseStamped pose;
           pose.header = global_path.header;
           pose.pose.position.z = 0.0;
          
           pose.pose = start.pose;
           // if we have a different start and goal orientation,   set the unique path pose to the goal
           // orientation,   unless use_final_approach_orientation=true where we need it to be the start
           // orientation to avoid movement from the local planner
           if (  start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_ ) {
           pose.pose.orientation = goal.pose.orientation;
           }
           global_path.poses.push_back(  pose );
           return global_path;
           }
          
           planner_->setStartAndGoal(  start,   goal );
           RCLCPP_DEBUG(  
           logger_,   "Got the src and dst... (  %i,   %i ) && (  %i,   %i )",  
           planner_->src_.x,   planner_->src_.y,   planner_->dst_.x,   planner_->dst_.y );
           getPlan(  global_path );
           // check if a plan is generated
           size_t plan_size = global_path.poses.size(   );
           if (  plan_size > 0 ) {
           global_path.poses.back(   ).pose.orientation = goal.pose.orientation;
           }
          
           // If use_final_approach_orientation=true,   interpolate the last pose orientation from the
           // previous pose to set the orientation to the 'final approach' orientation of the robot so
           // it does not rotate.
           // And deal with corner case of plan of length 1
           if (  use_final_approach_orientation_ ) {
           if (  plan_size == 1 ) {
           global_path.poses.back(   ).pose.orientation = start.pose.orientation;
           } else if (  plan_size > 1 ) {
           double dx,   dy,   theta;
           auto last_pose = global_path.poses.back(   ).pose.position;
           auto approach_pose = global_path.poses[plan_size - 2].pose.position;
           dx = last_pose.x - approach_pose.x;
           dy = last_pose.y - approach_pose.y;
           theta = atan2(  dy,   dx );
           global_path.poses.back(   ).pose.orientation =
           nav2_util::geometry_utils::orientationAroundZAxis(  theta );
           }
           }
          
           auto stop_time = std::chrono::steady_clock::now(   );
           auto dur = std::chrono::duration_cast<std::chrono::microseconds>(  stop_time - start_time );
           RCLCPP_DEBUG(  logger_,   "the time taken is : %i",   static_cast<int>(  dur.count(   ) ) );
           RCLCPP_DEBUG(  logger_,   "the nodes_opened are: %i",   planner_->nodes_opened );
           return global_path;
          }
          
     153  void ThetaStarPlanner::getPlan(  nav_msgs::msg::Path & global_path )
          {
           std::vector<coordsW> path;
           if (  planner_->isUnsafeToPlan(   ) ) {
           RCLCPP_ERROR(  logger_,   "Either of the start or goal pose are an obstacle! " );
           global_path.poses.clear(   );
           } else if (  planner_->generatePath(  path ) ) {
           global_path = linearInterpolation(  path,   planner_->costmap_->getResolution(   ) );
           } else {
           RCLCPP_ERROR(  logger_,   "Could not generate path between the given poses" );
           global_path.poses.clear(   );
           }
           global_path.header.stamp = clock_->now(   );
           global_path.header.frame_id = global_frame_;
          }
          
     169  nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation(  
     170   const std::vector<coordsW> & raw_path,  
           const double & dist_bw_points )
          {
           nav_msgs::msg::Path pa;
          
           geometry_msgs::msg::PoseStamped p1;
           for (  unsigned int j = 0; j < raw_path.size(   ) - 1; j++ ) {
           coordsW pt1 = raw_path[j];
           p1.pose.position.x = pt1.x;
           p1.pose.position.y = pt1.y;
           pa.poses.push_back(  p1 );
          
           coordsW pt2 = raw_path[j + 1];
           double distance = std::hypot(  pt2.x - pt1.x,   pt2.y - pt1.y );
           int loops = static_cast<int>(  distance / dist_bw_points );
           double sin_alpha = (  pt2.y - pt1.y ) / distance;
           double cos_alpha = (  pt2.x - pt1.x ) / distance;
           for (  int k = 1; k < loops; k++ ) {
           p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha;
           p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha;
           pa.poses.push_back(  p1 );
           }
           }
          
           return pa;
          }
          
          rcl_interfaces::msg::SetParametersResult
     198  ThetaStarPlanner::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == name_ + ".how_many_corners" ) {
           planner_->how_many_corners_ = parameter.as_int(   );
           }
           } else if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == name_ + ".w_euc_cost" ) {
           planner_->w_euc_cost_ = parameter.as_double(   );
           } else if (  name == name_ + ".w_traversal_cost" ) {
           planner_->w_traversal_cost_ = parameter.as_double(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == name_ + ".use_final_approach_orientation" ) {
           use_final_approach_orientation_ = parameter.as_bool(   );
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_theta_star_planner
          
          #include "pluginlib/class_list_macros.hpp"
     229  PLUGINLIB_EXPORT_CLASS(  nav2_theta_star_planner::ThetaStarPlanner,   nav2_core::GlobalPlanner )

navigation2/nav2_theta_star_planner/test/test_theta_star.cpp

       1  // Copyright 2020 Anshumaan Singh
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <vector>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_theta_star_planner/theta_star.hpp"
          #include "nav2_theta_star_planner/theta_star_planner.hpp"
          
      22  class init_rclcpp
          {
          public:
      25   init_rclcpp(   ) {rclcpp::init(  0,   nullptr );}
      26   ~init_rclcpp(   ) {rclcpp::shutdown(   );}
          };
          
          /// class created to access the protected members of the ThetaStar class
          /// u is used as shorthand for use
      31  class test_theta_star : public theta_star::ThetaStar
          {
          public:
      34   int getSizeOfNodePosition(   )
           {
           return static_cast<int>(  node_position_.size(   ) );
           }
          
      39   bool ulosCheck(  const int & x0,   const int & y0,   const int & x1,   const int & y1,   double & sl_cost )
           {
           return losCheck(  x0,   y0,   x1,   y1,   sl_cost );
           }
          
      44   bool uwithinLimits(  const int & cx,   const int & cy ) {return withinLimits(  cx,   cy );}
          
      46   bool uisGoal(  const tree_node & this_node ) {return isGoal(  this_node );}
          
      48   void uinitializePosn(  int size_inc = 0 )
           {
           node_position_.reserve(  size_x_ * size_y_ ); initializePosn(  size_inc );
           }
          
      53   void uaddIndex(  const int & cx,   const int & cy ) {addIndex(  cx,   cy,   &nodes_data_[0] );}
          
      55   tree_node * ugetIndex(  const int & cx,   const int & cy ) {return getIndex(  cx,   cy );}
          
      57   tree_node * test_getIndex(   ) {return &nodes_data_[0];}
          
      59   void uaddToNodesData(  const int & id ) {addToNodesData(  id );}
          
      61   void uresetContainers(   ) {nodes_data_.clear(   ); resetContainers(   );}
          
      63   bool runAlgo(  std::vector<coordsW> & path )
           {
           if (  !isUnsafeToPlan(   ) ) {
           return generatePath(  path );
           }
           return false;
           }
          };
          
          init_rclcpp node;
          
          // Tests meant to test the algorithm itself and its helper functions
      75  TEST(  ThetaStarTest,   test_theta_star ) {
           auto planner_ = std::make_unique<test_theta_star>(   );
           planner_->costmap_ = new nav2_costmap_2d::Costmap2D(  50,   50,   1.0,   0.0,   0.0,   0 );
           for (  int i = 7; i <= 14; i++ ) {
           for (  int j = 7; j <= 14; j++ ) {
           planner_->costmap_->setCost(  i,   j,   253 );
           }
           }
           coordsM s = {5,   5},   g = {18,   18};
           int size_x = 20,   size_y = 20;
           planner_->size_x_ = size_x;
           planner_->size_y_ = size_y;
           geometry_msgs::msg::PoseStamped start,   goal;
           start.pose.position.x = s.x;
           start.pose.position.y = s.y;
           start.pose.orientation.w = 1.0;
           goal.pose.position.x = g.x;
           goal.pose.position.y = g.y;
           goal.pose.orientation.w = 1.0;
           /// Check if the setStartAndGoal function works properly
           planner_->setStartAndGoal(  start,   goal );
           EXPECT_TRUE(  planner_->src_.x == s.x && planner_->src_.y == s.y );
           EXPECT_TRUE(  planner_->dst_.x == g.x && planner_->dst_.y == g.y );
          
           /// Check if the initializePosn function works properly
           planner_->uinitializePosn(  size_x * size_y );
           EXPECT_EQ(  planner_->getSizeOfNodePosition(   ),   (  size_x * size_y ) );
          
           /// Check if the withinLimits function works properly
           EXPECT_TRUE(  planner_->uwithinLimits(  18,   18 ) );
           EXPECT_FALSE(  planner_->uwithinLimits(  120,   140 ) );
          
           tree_node n = {g.x,   g.y,   120,   0,   NULL,   false,   20};
           n.parent_id = &n;
           /// Check if the isGoal function works properly
           EXPECT_TRUE(  planner_->uisGoal(  n ) ); // both (  x,  y ) are the goal coordinates
           n.x = 25;
           EXPECT_FALSE(  planner_->uisGoal(  n ) ); // only y coordinate matches with that of goal
           n.x = g.x;
           n.y = 20;
           EXPECT_FALSE(  planner_->uisGoal(  n ) ); // only x coordinate matches with that of goal
           n.x = 30;
           EXPECT_FALSE(  planner_->uisGoal(  n ) ); // both (  x,   y ) are different from the goal coordinate
          
           /// Check if the isSafe functions work properly
           EXPECT_TRUE(  planner_->isSafe(  5,   5 ) ); // cost at this point is 0
           EXPECT_FALSE(  planner_->isSafe(  10,   10 ) ); // cost at this point is 253 (  >LETHAL_COST )
          
           /// Check if the functions addIndex & getIndex work properly
           coordsM c = {20,   30};
           planner_->uaddToNodesData(  0 );
           planner_->uaddIndex(  c.x,   c.y );
           tree_node * c_node = planner_->ugetIndex(  c.x,   c.y );
           EXPECT_EQ(  c_node,   planner_->test_getIndex(   ) );
          
           double sl_cost = 0.0;
           /// Checking for the case where the losCheck should return the value as true
           EXPECT_TRUE(  planner_->ulosCheck(  2,   2,   7,   20,   sl_cost ) );
           /// and as false
           EXPECT_FALSE(  planner_->ulosCheck(  2,   2,   18,   18,   sl_cost ) );
          
           planner_->uresetContainers(   );
           std::vector<coordsW> path;
           /// Check if the planner returns a path for the case where a path exists
           EXPECT_TRUE(  planner_->runAlgo(  path ) );
           EXPECT_GT(  static_cast<int>(  path.size(   ) ),   0 );
           /// and where it doesn't exist
           path.clear(   );
           planner_->src_ = {10,   10};
           EXPECT_FALSE(  planner_->runAlgo(  path ) );
           EXPECT_EQ(  static_cast<int>(  path.size(   ) ),   0 );
          }
          
          // Smoke tests meant to detect issues arising from the plugin part rather than the algorithm
     149  TEST(  ThetaStarPlanner,   test_theta_star_planner ) {
           rclcpp_lifecycle::LifecycleNode::SharedPtr life_node =
           std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "ThetaStarPlannerTest" );
          
           std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
           std::make_shared<nav2_costmap_2d::Costmap2DROS>(  "global_costmap" );
           costmap_ros->on_configure(  rclcpp_lifecycle::State(   ) );
          
           geometry_msgs::msg::PoseStamped start,   goal;
           start.pose.position.x = 0.0;
           start.pose.position.y = 0.0;
           start.pose.orientation.w = 1.0;
           goal = start;
           auto planner_2d = std::make_unique<nav2_theta_star_planner::ThetaStarPlanner>(   );
           planner_2d->configure(  life_node,   "test",   nullptr,   costmap_ros );
           planner_2d->activate(   );
          
           nav_msgs::msg::Path path = planner_2d->createPlan(  start,   goal );
           EXPECT_GT(  static_cast<int>(  path.poses.size(   ) ),   0 );
          
           // test if the goal is unsafe
           for (  int i = 7; i <= 14; i++ ) {
           for (  int j = 7; j <= 14; j++ ) {
           costmap_ros->getCostmap(   )->setCost(  i,   j,   254 );
           }
           }
           goal.pose.position.x = 1.0;
           goal.pose.position.y = 1.0;
           path = planner_2d->createPlan(  start,   goal );
           EXPECT_EQ(  static_cast<int>(  path.poses.size(   ) ),   0 );
          
           planner_2d->deactivate(   );
           planner_2d->cleanup(   );
          
           planner_2d.reset(   );
           costmap_ros->on_cleanup(  rclcpp_lifecycle::State(   ) );
           life_node.reset(   );
           costmap_ros.reset(   );
          }

navigation2/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
          #define NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
          
          #include <string>
          #include "nav2_util/service_client.hpp"
          #include "std_srvs/srv/empty.hpp"
          #include "nav2_msgs/srv/clear_entire_costmap.hpp"
          
          namespace nav2_util
          {
          /**
           * @class nav2_util::ClearEntirelyCostmapServiceClient
           * @brief A service client to clear costmaps entirely
           */
      29  class ClearEntirelyCostmapServiceClient
      30   : public nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>
          {
          public:
           /**
          * @brief A constructor for nav2_util::ClearEntirelyCostmapServiceClient
           */
      36   explicit ClearEntirelyCostmapServiceClient(  const std::string & service_name )
           : nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>(  service_name )
           {
           }
          
           using clearEntirelyCostmapServiceRequest =
           nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::RequestType;
           using clearEntirelyCostmapServiceResponse =
           nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::ResponseType;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/costmap.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__COSTMAP_HPP_
          #define NAV2_UTIL__COSTMAP_HPP_
          
          #include <vector>
          #include <cstdint>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_msgs/msg/costmap.hpp"
          #include "nav2_msgs/msg/costmap_meta_data.hpp"
          #include "nav_msgs/msg/occupancy_grid.hpp"
          
          namespace nav2_util
          {
          
      29  enum class TestCostmap
          {
           open_space,  
           bounded,  
           bottom_left_obstacle,  
           top_left_obstacle,  
           maze1,  
           maze2
          };
          
          /**
           * @class nav2_util::Costmap
           * @brief Class for a single layered costmap initialized from an
           * occupancy grid representing the map.
           */
      44  class Costmap
          {
          public:
           typedef uint8_t CostValue;
          
           /**
           * @brief A constructor for nav2_util::Costmap
           * @param node Ptr to a node
           * @param trinary_costmap Whether the costmap should be trinary
           * @param track_unknown_space Whether to track unknown space in costmap
           * @param lethal_threshold The lethal space cost threshold to use
           * @param unknown_cost_value Internal costmap cell value for unknown space
           */
      57   Costmap(  
      58   rclcpp::Node * node,   bool trinary_costmap = true,   bool track_unknown_space = true,  
           int lethal_threshold = 100,   int unknown_cost_value = -1 );
           Costmap(   ) = delete;
           ~Costmap(   );
          
           /**
           * @brief Set the static map of this costmap
           * @param occupancy_grid Occupancy grid to populate this costmap with
           */
           void set_static_map(  const nav_msgs::msg::OccupancyGrid & occupancy_grid );
          
           /**
           * @brief Set the test costmap type of this costmap
           * @param testCostmapType Type of stored costmap to use
           */
           void set_test_costmap(  const TestCostmap & testCostmapType );
          
           /**
           * @brief Get a costmap message from this object
           * @param specifications Parameters of costmap
           * @return Costmap msg of this costmap
           */
           nav2_msgs::msg::Costmap get_costmap(  const nav2_msgs::msg::CostmapMetaData & specifications );
          
           /**
           * @brief Get a metadata message from this object
           * @return Costmap metadata of this costmap
           */
           nav2_msgs::msg::CostmapMetaData get_properties(   ) {return costmap_properties_;}
          
           /**
           * @brief Get whether some coordinates are free
           * @return bool if free
           */
           bool is_free(  const unsigned int x_coordinate,   const unsigned int y_coordinate ) const;
          
           /**
           * @brief Get whether some index in the costmap is free
           * @return bool if free
           */
      98   bool is_free(  const unsigned int index ) const;
          
           // Mapping for often used cost values
           static const CostValue no_information;
           static const CostValue lethal_obstacle;
           static const CostValue inscribed_inflated_obstacle;
           static const CostValue medium_cost;
           static const CostValue free_space;
          
          private:
           /**
           * @brief Get data from the test
           * @return data
           */
     112   std::vector<uint8_t> get_test_data(  const TestCostmap configuration );
          
           /**
           * @brief Get the interpreted value in the costmap
           * @return uint value
           */
     118   uint8_t interpret_value(  const int8_t value ) const;
          
           // Costmap isn't itself a node
           rclcpp::Node * node_;
          
           // TODO(  orduno ): For now,   only holding costs from static map
           nav2_msgs::msg::CostmapMetaData costmap_properties_;
           std::vector<uint8_t> costs_;
          
           // Static layer parameters
           bool trinary_costmap_;
           bool track_unknown_space_;
           int lethal_threshold_;
           int unknown_cost_value_;
          
           // Flags to determine the origin of the costmap
           bool map_provided_;
           bool using_test_map_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__COSTMAP_HPP_

navigation2/nav2_util/include/nav2_util/execution_timer.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__EXECUTION_TIMER_HPP_
          #define NAV2_UTIL__EXECUTION_TIMER_HPP_
          
          #include <chrono>
          
          namespace nav2_util
          {
          
          /// @brief Measures execution time of code between calls to start and end
      24  class ExecutionTimer
          {
          public:
           using Clock = std::chrono::high_resolution_clock;
           using nanoseconds = std::chrono::nanoseconds;
          
           /// @brief Call just prior to code you want to measure
           void start(   ) {start_ = Clock::now(   );}
          
           /// @brief Call just after the code you want to measure
           void end(   ) {end_ = Clock::now(   );}
          
           /// @brief Extract the measured time as an integral std::chrono::duration object
      37   nanoseconds elapsed_time(   ) {return end_ - start_;}
          
           /// @brief Extract the measured time as a floating point number of seconds.
      40   double elapsed_time_in_seconds(   )
           {
           return std::chrono::duration<double>(  end_ - start_ ).count(   );
           }
          
          protected:
           Clock::time_point start_;
           Clock::time_point end_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__EXECUTION_TIMER_HPP_

navigation2/nav2_util/include/nav2_util/geometry_utils.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_
          #define NAV2_UTIL__GEOMETRY_UTILS_HPP_
          
          #include <cmath>
          
          #include "geometry_msgs/msg/pose.hpp"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "geometry_msgs/msg/pose2_d.hpp"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/quaternion.hpp"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "nav_msgs/msg/path.hpp"
          
          namespace nav2_util
          {
          namespace geometry_utils
          {
          
          /**
           * @brief Get a geometry_msgs Quaternion from a yaw angle
           * @param angle Yaw angle to generate a quaternion from
           * @return geometry_msgs Quaternion
           */
      38  inline geometry_msgs::msg::Quaternion orientationAroundZAxis(  double angle )
          {
           tf2::Quaternion q;
           q.setRPY(  0,   0,   angle ); // void returning function
           return tf2::toMsg(  q );
          }
          
          /**
           * @brief Get the euclidean distance between 2 geometry_msgs::Points
           * @param pos1 First point
           * @param pos1 Second point
           * @param is_3d True if a true L2 distance is desired (  default false )
           * @return double L2 distance
           */
      52  inline double euclidean_distance(  
           const geometry_msgs::msg::Point & pos1,  
           const geometry_msgs::msg::Point & pos2,  
           const bool is_3d = false )
          {
           double dx = pos1.x - pos2.x;
           double dy = pos1.y - pos2.y;
          
           if (  is_3d ) {
           double dz = pos1.z - pos2.z;
           return std::hypot(  dx,   dy,   dz );
           }
          
           return std::hypot(  dx,   dy );
          }
          
          /**
           * @brief Get the L2 distance between 2 geometry_msgs::Poses
           * @param pos1 First pose
           * @param pos1 Second pose
           * @param is_3d True if a true L2 distance is desired (  default false )
           * @return double euclidean distance
           */
      75  inline double euclidean_distance(  
           const geometry_msgs::msg::Pose & pos1,  
           const geometry_msgs::msg::Pose & pos2,  
           const bool is_3d = false )
          {
           double dx = pos1.position.x - pos2.position.x;
           double dy = pos1.position.y - pos2.position.y;
          
           if (  is_3d ) {
           double dz = pos1.position.z - pos2.position.z;
           return std::hypot(  dx,   dy,   dz );
           }
          
           return std::hypot(  dx,   dy );
          }
          
          /**
           * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
           * @param pos1 First pose
           * @param pos1 Second pose
           * @param is_3d True if a true L2 distance is desired (  default false )
           * @return double L2 distance
           */
      98  inline double euclidean_distance(  
           const geometry_msgs::msg::PoseStamped & pos1,  
           const geometry_msgs::msg::PoseStamped & pos2,  
           const bool is_3d = false )
          {
           return euclidean_distance(  pos1.pose,   pos2.pose,   is_3d );
          }
          
          /**
           * @brief Get the L2 distance between 2 geometry_msgs::Pose2D
           * @param pos1 First pose
           * @param pos1 Second pose
           * @return double L2 distance
           */
     112  inline double euclidean_distance(  
           const geometry_msgs::msg::Pose2D & pos1,  
           const geometry_msgs::msg::Pose2D & pos2 )
          {
           double dx = pos1.x - pos2.x;
           double dy = pos1.y - pos2.y;
          
           return std::hypot(  dx,   dy );
          }
          
          /**
           * Find element in iterator with the minimum calculated value
           */
          template<typename Iter,   typename Getter>
     126  inline Iter min_by(  Iter begin,   Iter end,   Getter getCompareVal )
          {
           if (  begin == end ) {
           return end;
           }
           auto lowest = getCompareVal(  *begin );
           Iter lowest_it = begin;
           for (  Iter it = ++begin; it != end; ++it ) {
           auto comp = getCompareVal(  *it );
           if (  comp < lowest ) {
           lowest = comp;
           lowest_it = it;
           }
           }
           return lowest_it;
          }
          
          /**
           * Find first element in iterator that is greater integrated distance than comparevalue
           */
          template<typename Iter,   typename Getter>
     147  inline Iter first_after_integrated_distance(  Iter begin,   Iter end,   Getter getCompareVal )
          {
           if (  begin == end ) {
           return end;
           }
           Getter dist = 0.0;
           for (  Iter it = begin; it != end - 1; it++ ) {
           dist += euclidean_distance(  *it,   *(  it + 1 ) );
           if (  dist > getCompareVal ) {
           return it + 1;
           }
           }
           return end;
          }
          
          /**
           * @brief Calculate the length of the provided path,   starting at the provided index
           * @param path Path containing the poses that are planned
           * @param start_index Optional argument specifying the starting index for
           * the calculation of path length. Provide this if you want to calculate length of a
           * subset of the path.
           * @return double Path length
           */
     170  inline double calculate_path_length(  const nav_msgs::msg::Path & path,   size_t start_index = 0 )
          {
           if (  start_index + 1 >= path.poses.size(   ) ) {
           return 0.0;
           }
           double path_length = 0.0;
           for (  size_t idx = start_index; idx < path.poses.size(   ) - 1; ++idx ) {
           path_length += euclidean_distance(  path.poses[idx].pose,   path.poses[idx + 1].pose );
           }
           return path_length;
          }
          
          } // namespace geometry_utils
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__GEOMETRY_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_node.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__LIFECYCLE_NODE_HPP_
          #define NAV2_UTIL__LIFECYCLE_NODE_HPP_
          
          #include <memory>
          #include <string>
          #include <thread>
          
          #include "nav2_util/node_thread.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "bondcpp/bond.hpp"
          #include "bond/msg/constants.hpp"
          
          namespace nav2_util
          {
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          /**
           * @class nav2_util::LifecycleNode
           * @brief A lifecycle node wrapper to enable common Nav2 needs such as manipulating parameters
           */
      37  class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
          {
          public:
           /**
           * @brief A lifecycle node constructor
           * @param node_name Name for the node
           * @param namespace Namespace for the node,   if any
           * @param options Node options
           */
      46   LifecycleNode(  
           const std::string & node_name,  
           const std::string & ns = "",  
           const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
      50   virtual ~LifecycleNode(   );
          
           typedef struct
           {
           double from_value;
           double to_value;
           double step;
           } floating_point_range;
          
           typedef struct
           {
           int from_value;
           int to_value;
           int step;
           } integer_range;
          
           /**
           * @brief Declare a parameter that has no integer or floating point range constraints
           * @param node_name Name of parameter
           * @param default_value Default node value to add
           * @param description Node description
           * @param additional_constraints Any additional constraints on the parameters to list
           * @param read_only Whether this param should be considered read only
           */
      74   void add_parameter(  
      75   const std::string & name,   const rclcpp::ParameterValue & default_value,  
      76   const std::string & description = "",   const std::string & additional_constraints = "",  
      77   bool read_only = false )
           {
           auto descriptor = rcl_interfaces::msg::ParameterDescriptor(   );
          
           descriptor.name = name;
           descriptor.description = description;
           descriptor.additional_constraints = additional_constraints;
           descriptor.read_only = read_only;
          
           declare_parameter(  descriptor.name,   default_value,   descriptor );
           }
          
           /**
           * @brief Declare a parameter that has a floating point range constraint
           * @param node_name Name of parameter
           * @param default_value Default node value to add
           * @param fp_range floating point range
           * @param description Node description
           * @param additional_constraints Any additional constraints on the parameters to list
           * @param read_only Whether this param should be considered read only
           */
      98   void add_parameter(  
      99   const std::string & name,   const rclcpp::ParameterValue & default_value,  
     100   const floating_point_range fp_range,  
     101   const std::string & description = "",   const std::string & additional_constraints = "",  
     102   bool read_only = false )
           {
           auto descriptor = rcl_interfaces::msg::ParameterDescriptor(   );
          
           descriptor.name = name;
           descriptor.description = description;
           descriptor.additional_constraints = additional_constraints;
           descriptor.read_only = read_only;
           descriptor.floating_point_range.resize(  1 );
           descriptor.floating_point_range[0].from_value = fp_range.from_value;
           descriptor.floating_point_range[0].to_value = fp_range.to_value;
           descriptor.floating_point_range[0].step = fp_range.step;
          
           declare_parameter(  descriptor.name,   default_value,   descriptor );
           }
          
           /**
           * @brief Declare a parameter that has an integer range constraint
           * @param node_name Name of parameter
           * @param default_value Default node value to add
           * @param integer_range Integer range
           * @param description Node description
           * @param additional_constraints Any additional constraints on the parameters to list
           * @param read_only Whether this param should be considered read only
           */
     127   void add_parameter(  
     128   const std::string & name,   const rclcpp::ParameterValue & default_value,  
     129   const integer_range int_range,  
     130   const std::string & description = "",   const std::string & additional_constraints = "",  
     131   bool read_only = false )
           {
           auto descriptor = rcl_interfaces::msg::ParameterDescriptor(   );
          
           descriptor.name = name;
           descriptor.description = description;
           descriptor.additional_constraints = additional_constraints;
           descriptor.read_only = read_only;
           descriptor.integer_range.resize(  1 );
           descriptor.integer_range[0].from_value = int_range.from_value;
           descriptor.integer_range[0].to_value = int_range.to_value;
           descriptor.integer_range[0].step = int_range.step;
          
           declare_parameter(  descriptor.name,   default_value,   descriptor );
           }
          
           /**
           * @brief Get a shared pointer of this
           */
     150   std::shared_ptr<nav2_util::LifecycleNode> shared_from_this(   )
           {
           return std::static_pointer_cast<nav2_util::LifecycleNode>(  
           rclcpp_lifecycle::LifecycleNode::shared_from_this(   ) );
           }
          
           /**
           * @brief Abstracted on_error state transition callback,   since unimplemented as of 2020
           * in the managed ROS2 node state machine
           * @param state State prior to error transition
           * @return Return type for success or failed transition to error state
           */
     162   nav2_util::CallbackReturn on_error(  const rclcpp_lifecycle::State & /*state*/ )
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Lifecycle node %s does not have error state implemented",   get_name(   ) );
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
           /**
           * @brief Create bond connection to lifecycle manager
           */
     173   void createBond(   );
          
           /**
           * @brief Destroy bond connection to lifecycle manager
           */
     178   void destroyBond(   );
          
          protected:
           /**
           * @brief Print notifications for lifecycle node
           */
     184   void printLifecycleNodeNotification(   );
          
           // Connection to tell that server is still up
           std::unique_ptr<bond::Bond> bond_{nullptr};
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__LIFECYCLE_NODE_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_service_client.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
          #define NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
          
          #include <chrono>
          #include <memory>
          #include <string>
          
          #include "lifecycle_msgs/srv/change_state.hpp"
          #include "lifecycle_msgs/srv/get_state.hpp"
          #include "nav2_util/service_client.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_util
          {
          
          /// Helper functions to interact with a lifecycle node.
      31  class LifecycleServiceClient
          {
          public:
      34   explicit LifecycleServiceClient(  const std::string & lifecycle_node_name );
      35   LifecycleServiceClient(  
      36   const std::string & lifecycle_node_name,  
      37   rclcpp::Node::SharedPtr parent_node );
          
           /// Trigger a state change
           /**
           * Throws std::runtime_error on failure
           */
      43   bool change_state(  
      44   const uint8_t transition,   // takes a lifecycle_msgs::msg::Transition id
      45   const std::chrono::seconds timeout );
          
           /// Trigger a state change,   returning result
      48   bool change_state(  std::uint8_t transition );
          
           /// Get the current state as a lifecycle_msgs::msg::State id value
           /**
           * Throws std::runtime_error on failure
           */
      54   uint8_t get_state(  const std::chrono::seconds timeout = std::chrono::seconds(  2 ) );
          
          protected:
      57   rclcpp::Node::SharedPtr node_;
      58   ServiceClient<lifecycle_msgs::srv::ChangeState> change_state_;
      59   ServiceClient<lifecycle_msgs::srv::GetState> get_state_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_utils.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__LIFECYCLE_UTILS_HPP_
          #define NAV2_UTIL__LIFECYCLE_UTILS_HPP_
          
          #include <vector>
          #include <string>
          #include <chrono>
          #include "nav2_util/string_utils.hpp"
          
          namespace nav2_util
          {
          
          /// Transition the given lifecycle nodes to the ACTIVATED state in order
          /** At this time,   service calls frequently hang for unknown reasons. The only
           * way to combat that is to timeout the service call and retry it. To use this
           * function,   estimate how long your nodes should take to at each transition and
           * set your timeout accordingly.
           * \param[in] node_names A vector of the fully qualified node names to startup.
           * \param[in] service_call_timeout The maximum amount of time to wait for a
           * service call.
           * \param[in] retries The number of times to try a state transition service call
           */
      36  void startup_lifecycle_nodes(  
           const std::vector<std::string> & node_names,  
           const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(   ),  
           const int retries = 3 );
          
          /// Transition the given lifecycle nodes to the ACTIVATED state in order.
          /**
           * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
           */
      45  void startup_lifecycle_nodes(  
           const std::string & nodes,  
           const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(   ),  
           const int retries = 3 )
          {
           startup_lifecycle_nodes(  split(  nodes,   ':' ),   service_call_timeout,   retries );
          }
          
          /// Transition the given lifecycle nodes to the UNCONFIGURED state in order
          /** At this time,   service calls frequently hang for unknown reasons. The only
           * way to combat that is to timeout the service call and retry it. To use this
           * function,   estimate how long your nodes should take to at each transition and
           * set your timeout accordingly.
           * \param[in] node_names A vector of the fully qualified node names to reset.
           * \param[in] service_call_timeout The maximum amount of time to wait for a
           * service call.
           * \param[in] retries The number of times to try a state transition service call
           */
      63  void reset_lifecycle_nodes(  
           const std::vector<std::string> & node_names,  
           const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(   ),  
           const int retries = 3 );
          
          /// Transition the given lifecycle nodes to the UNCONFIGURED state in order.
          /**
           * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
           */
      72  void reset_lifecycle_nodes(  
           const std::string & nodes,  
           const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(   ),  
           const int retries = 3 )
          {
           reset_lifecycle_nodes(  split(  nodes,   ':' ),   service_call_timeout,   retries );
          }
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__LIFECYCLE_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/line_iterator.hpp

       1  // Copyright (  c ) 2012,   Willow Garage,   Inc.
          // All rights reserved.
          //
          // Software License Agreement (  BSD License 2.0 )
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions
          // are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          // * Redistributions in binary form must reproduce the above
          // copyright notice,   this list of conditions and the following
          // disclaimer in the documentation and/or other materials provided
          // with the distribution.
          // * Neither the name of the Willow Garage,   Inc. nor the names of its
          // contributors may be used to endorse or promote products derived
          // from this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          // LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          // INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          // BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          // LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          // CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          // LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          // ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          #ifndef NAV2_UTIL__LINE_ITERATOR_HPP_
          #define NAV2_UTIL__LINE_ITERATOR_HPP_
          
          #include <stdlib.h>
          
          namespace nav2_util
          {
          
          
          /**
           * @class nav2_util::LineIterator
           * @brief An iterator implementing Bresenham Ray-Tracing.
           */
      46  class LineIterator
          {
          public:
           /**
           * @brief A constructor for LineIterator
           * @param x0 Starting x
           * @param y0 Starting y
           * @param x1 Ending x
           * @param y1 Ending y
           */
      56   LineIterator(  int x0,   int y0,   int x1,   int y1 )
           : x0_(  x0 ),  
           y0_(  y0 ),  
           x1_(  x1 ),  
           y1_(  y1 ),  
           x_(  x0 ),   // X and Y start of at first endpoint.
           y_(  y0 ),  
           deltax_(  abs(  x1 - x0 ) ),  
           deltay_(  abs(  y1 - y0 ) ),  
           curpixel_(  0 )
           {
           if (  x1_ >= x0_ ) { // The x-values are increasing
           xinc1_ = 1;
           xinc2_ = 1;
           } else { // The x-values are decreasing
           xinc1_ = -1;
           xinc2_ = -1;
           }
          
           if (  y1_ >= y0_ ) { // The y-values are increasing
           yinc1_ = 1;
           yinc2_ = 1;
           } else { // The y-values are decreasing
           yinc1_ = -1;
           yinc2_ = -1;
           }
          
           if (  deltax_ >= deltay_ ) { // There is at least one x-value for every y-value
           xinc1_ = 0; // Don't change the x when numerator >= denominator
           yinc2_ = 0; // Don't change the y for every iteration
           den_ = deltax_;
           num_ = deltax_ / 2;
           numadd_ = deltay_;
           numpixels_ = deltax_; // There are more x-values than y-values
           } else { // There is at least one y-value for every x-value
           xinc2_ = 0; // Don't change the x for every iteration
           yinc1_ = 0; // Don't change the y when numerator >= denominator
           den_ = deltay_;
           num_ = deltay_ / 2;
           numadd_ = deltax_;
           numpixels_ = deltay_; // There are more y-values than x-values
           }
           }
          
           /**
           * @brief If the iterator is valid
           * @return bool If valid
           */
     104   bool isValid(   ) const
           {
           return curpixel_ <= numpixels_;
           }
          
           /**
           * @brief Advance iteration along the line
           */
     112   void advance(   )
           {
           num_ += numadd_; // Increase the numerator by the top of the fraction
           if (  num_ >= den_ ) { // Check if numerator >= denominator
           num_ -= den_; // Calculate the new numerator value
           x_ += xinc1_; // Change the x as appropriate
           y_ += yinc1_; // Change the y as appropriate
           }
           x_ += xinc2_; // Change the x as appropriate
           y_ += yinc2_; // Change the y as appropriate
          
           curpixel_++;
           }
          
           /**
           * @brief Get current X value
           * @return X
           */
     130   int getX(   ) const
           {
           return x_;
           }
          
           /**
           * @brief Get current Y value
           * @return Y
           */
     139   int getY(   ) const
           {
           return y_;
           }
          
           /**
           * @brief Get initial X value
           * @return X
           */
     148   int getX0(   ) const
           {
           return x0_;
           }
          
           /**
           * @brief Get initial Y value
           * @return Y
           */
     157   int getY0(   ) const
           {
           return y0_;
           }
          
           /**
           * @brief Get terminal X value
           * @return X
           */
     166   int getX1(   ) const
           {
           return x1_;
           }
          
           /**
           * @brief Get terminal Y value
           * @return Y
           */
     175   int getY1(   ) const
           {
           return y1_;
           }
          
          private:
           int x0_; ///< X coordinate of first end point.
           int y0_; ///< Y coordinate of first end point.
           int x1_; ///< X coordinate of second end point.
           int y1_; ///< Y coordinate of second end point.
          
           int x_; ///< X coordinate of current point.
           int y_; ///< Y coordinate of current point.
          
           int deltax_; ///< Difference between Xs of endpoints.
           int deltay_; ///< Difference between Ys of endpoints.
          
           int curpixel_; ///< index of current point in line loop.
          
           int xinc1_,   xinc2_,   yinc1_,   yinc2_;
           int den_,   num_,   numadd_,   numpixels_;
          };
          
          } // end namespace nav2_util
          
          #endif // NAV2_UTIL__LINE_ITERATOR_HPP_

navigation2/nav2_util/include/nav2_util/node_thread.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__NODE_THREAD_HPP_
          #define NAV2_UTIL__NODE_THREAD_HPP_
          
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_util
          {
          /**
           * @class nav2_util::NodeThread
           * @brief A background thread to process node/executor callbacks
           */
      28  class NodeThread
          {
          public:
           /**
           * @brief A background thread to process node callbacks constructor
           * @param node_base Interface to Node to spin in thread
           */
      35   explicit NodeThread(  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base );
          
           /**
           * @brief A background thread to process executor's callbacks constructor
           * @param executor Interface to executor to spin in thread
           */
      41   explicit NodeThread(  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor );
          
           /**
           * @brief A background thread to process node callbacks constructor
           * @param node Node pointer to spin in thread
           */
           template<typename NodeT>
           explicit NodeThread(  NodeT node )
           : NodeThread(  node->get_node_base_interface(   ) )
           {}
          
           /**
           * @brief A destructor
           */
           ~NodeThread(   );
          
          protected:
           rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_;
           std::unique_ptr<std::thread> thread_;
           rclcpp::Executor::SharedPtr executor_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__NODE_THREAD_HPP_

navigation2/nav2_util/include/nav2_util/node_utils.hpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__NODE_UTILS_HPP_
          #define NAV2_UTIL__NODE_UTILS_HPP_
          
          #include <string>
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_util
          {
          
          /// Replace invalid characters in a potential node name
          /**
           * There is frequently a need to create internal nodes. They must have a name,  
           * and commonly the name is based on some parameter related to the node's
           * purpose. However,   only alphanumeric characters and '_' are allowed in node
           * names. This function replaces any invalid character with a '_'
           *
           * \param[in] potential_node_name Potential name but possibly with invalid charaters.
           * \return A copy of the input string but with non-alphanumeric characters replaced with '_'
           */
      34  std::string sanitize_node_name(  const std::string & potential_node_name );
          
          /// Concatenate two namespaces to produce an absolute namespace
          /**
           * \param[in] top_ns The namespace to place first
           * \param[in] sub_ns The namespace to place after top_ns
           * \return An absolute namespace starting with "/"
          */
      42  std::string add_namespaces(  const std::string & top_ns,   const std::string & sub_ns = "" );
          
          /// Add some random characters to a node name to ensure it is unique in the system
          /**
           * There are utility classes that create an internal private node to interact
           * with the system. These private nodes are given a generated name. If multiple
           * clients end up using the same service,   there is the potential for node name
           * conflicts. To ensure node names are globally unique,   this appends some random
           * numbers to the end of the prefix.
           *
           * \param[in] prefix A string to help understand the purpose of the node.
           * \return A copy of the prefix + '_' + 8 random digits. eg. prefix_12345678
           */
      55  std::string generate_internal_node_name(  const std::string & prefix = "" );
          
          /// Creates a node with a name as generated by generate_internal_node_name
          /**
           * Creates a node with the following settings:
           * - name generated by generate_internal_node_name
           * - no parameter services
           * - no parameter event publisher
           *
           * \param[in] prefix A string to help understand the purpose of the node.
           * \return A shared_ptr to the node.
           */
      67  rclcpp::Node::SharedPtr generate_internal_node(  const std::string & prefix = "" );
          
          /// Generates a pseudo random string of digits.
          /**
           * Generates pseudo random digits by converting the current system time to a
           * string. This means that any length more than 8 or so digits will just get
           * padded with zeros and doesn't add any additional randomness.
           *
           * \param[in] len Length of the output string
           * \return A string containing random digits
           */
      78  std::string time_to_string(  size_t len );
          
          rclcpp::NodeOptions
      81  get_node_options_default(  bool allow_undeclared = true,   bool declare_initial_params = true );
          
          /// Declares static ROS2 parameter and sets it to a given value if it was not already declared
          /* Declares static ROS2 parameter and sets it to a given value
           * if it was not already declared.
           *
           * \param[in] node A node in which given parameter to be declared
           * \param[in] param_name The name of parameter
           * \param[in] default_value Parameter value to initialize with
           * \param[in] parameter_descriptor Parameter descriptor (  optional )
           */
          template<typename NodeT>
      93  void declare_parameter_if_not_declared(  
           NodeT node,  
           const std::string & param_name,  
           const rclcpp::ParameterValue & default_value,  
           const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
           rcl_interfaces::msg::ParameterDescriptor(   ) )
          {
           if (  !node->has_parameter(  param_name ) ) {
           node->declare_parameter(  param_name,   default_value,   parameter_descriptor );
           }
          }
          
          /// Declares static ROS2 parameter with given type if it was not already declared
          /* Declares static ROS2 parameter with given type if it was not already declared.
           *
           * \param[in] node A node in which given parameter to be declared
           * \param[in] param_type The type of parameter
           * \param[in] default_value Parameter value to initialize with
           * \param[in] parameter_descriptor Parameter descriptor (  optional )
           */
          template<typename NodeT>
     114  void declare_parameter_if_not_declared(  
           NodeT node,  
           const std::string & param_name,  
           const rclcpp::ParameterType & param_type,  
           const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
           rcl_interfaces::msg::ParameterDescriptor(   ) )
          {
           if (  !node->has_parameter(  param_name ) ) {
           node->declare_parameter(  param_name,   param_type,   parameter_descriptor );
           }
          }
          
          /// Gets the type of plugin for the selected node and its plugin
          /**
           * Gets the type of plugin for the selected node and its plugin.
           * Actually seeks for the value of "<plugin_name>.plugin" parameter.
           *
           * \param[in] node Selected node
           * \param[in] plugin_name The name of plugin the type of which is being searched for
           * \return A string containing the type of plugin (  the value of "<plugin_name>.plugin" parameter )
           */
          template<typename NodeT>
     136  std::string get_plugin_type_param(  
           NodeT node,  
           const std::string & plugin_name )
          {
           declare_parameter_if_not_declared(  node,   plugin_name + ".plugin",   rclcpp::PARAMETER_STRING );
           std::string plugin_type;
           try {
           if (  !node->get_parameter(  plugin_name + ".plugin",   plugin_type ) ) {
           RCLCPP_FATAL(  
           node->get_logger(   ),   "Can not get 'plugin' param value for %s",   plugin_name.c_str(   ) );
           exit(  -1 );
           }
           } catch (  rclcpp::exceptions::ParameterUninitializedException & ex ) {
           RCLCPP_FATAL(  node->get_logger(   ),   "'plugin' param not defined for %s",   plugin_name.c_str(   ) );
           exit(  -1 );
           }
          
           return plugin_type;
          }
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__NODE_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/occ_grid_values.hpp

       1  // Copyright (  c ) 2020 Samsung Research Russia
          // All rights reserved.
          //
          // Software License Agreement (  BSD License 2.0 )
          //
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions
          // are met:
          //
          // * Redistributions of source code must retain the above copyright
          // notice,   this list of conditions and the following disclaimer.
          // * Redistributions in binary form must reproduce the above
          // copyright notice,   this list of conditions and the following
          // disclaimer in the documentation and/or other materials provided
          // with the distribution.
          // * Neither the name of the <ORGANIZATION> nor the names of its
          // contributors may be used to endorse or promote products derived
          // from this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          // LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          // INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          // BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          // LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          // CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          // LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          // ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          //
          // Author: Alexey Merzlyakov
          
          #ifndef NAV2_UTIL__OCC_GRID_VALUES_HPP_
          #define NAV2_UTIL__OCC_GRID_VALUES_HPP_
          
          namespace nav2_util
          {
          
          /**
           * @brief OccupancyGrid data constants
           */
          static constexpr int8_t OCC_GRID_UNKNOWN = -1;
          static constexpr int8_t OCC_GRID_FREE = 0;
          static constexpr int8_t OCC_GRID_OCCUPIED = 100;
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__OCC_GRID_VALUES_HPP_

navigation2/nav2_util/include/nav2_util/odometry_utils.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__ODOMETRY_UTILS_HPP_
          #define NAV2_UTIL__ODOMETRY_UTILS_HPP_
          
          #include <cmath>
          #include <chrono>
          #include <memory>
          #include <mutex>
          #include <string>
          #include <deque>
          
          #include "geometry_msgs/msg/twist.hpp"
          #include "geometry_msgs/msg/twist_stamped.hpp"
          #include "nav_msgs/msg/odometry.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_util
          {
          
          /**
           * @class OdomSmoother
           * Wrapper for getting smooth odometry readings using a simple moving avergae.
           * Subscribes to the topic with a mutex.
           */
      41  class OdomSmoother
          {
          public:
           /**
           * @brief Constructor that subscribes to an Odometry topic
           * @param parent NodeHandle for creating subscriber
           * @param filter_duration Duration for odom history (  seconds )
           * @param odom_topic Topic on which odometry should be received
           */
      50   explicit OdomSmoother(  
      51   const rclcpp::Node::WeakPtr & parent,  
           double filter_duration = 0.3,  
      53   const std::string & odom_topic = "odom" );
          
           /**
           * @brief Overloadded Constructor for nav_util::LifecycleNode parent
           * that subscribes to an Odometry topic
           * @param parent NodeHandle for creating subscriber
           * @param filter_duration Duration for odom history (  seconds )
           * @param odom_topic Topic on which odometry should be received
           */
      62   explicit OdomSmoother(  
      63   const nav2_util::LifecycleNode::WeakPtr & parent,  
           double filter_duration = 0.3,  
      65   const std::string & odom_topic = "odom" );
          
           /**
           * @brief Get twist msg from smoother
           * @return twist Twist msg
           */
      71   inline geometry_msgs::msg::Twist getTwist(   ) {return vel_smooth_.twist;}
          
           /**
           * @brief Get twist stamped msg from smoother
           * @return twist TwistStamped msg
           */
      77   inline geometry_msgs::msg::TwistStamped getTwistStamped(   ) {return vel_smooth_;}
          
          protected:
           /**
           * @brief Callback of odometry subscriber to process
           * @param msg Odometry msg to smooth
           */
      84   void odomCallback(  nav_msgs::msg::Odometry::SharedPtr msg );
          
           /**
           * @brief Update internal state of the smoother after getting new data
           */
      89   void updateState(   );
          
      91   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
      92   nav_msgs::msg::Odometry odom_cumulate_;
      93   geometry_msgs::msg::TwistStamped vel_smooth_;
      94   std::mutex odom_mutex_;
          
      96   rclcpp::Duration odom_history_duration_;
      97   std::deque<nav_msgs::msg::Odometry> odom_history_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__ODOMETRY_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/robot_utils.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2019 Steven Macenski
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__ROBOT_UTILS_HPP_
          #define NAV2_UTIL__ROBOT_UTILS_HPP_
          
          #include <string>
          
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "tf2_ros/buffer.h"
          #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_util
          {
          /**
          * @brief get the current pose of the robot
          * @param global_pose Pose to transform
          * @param tf_buffer TF buffer to use for the transformation
          * @param global_frame Frame to transform into
          * @param robot_frame Frame to transform from
          * @param transform_timeout TF Timeout to use for transformation
          * @return bool Whether it could be transformed successfully
          */
      38  bool getCurrentPose(  
           geometry_msgs::msg::PoseStamped & global_pose,  
           tf2_ros::Buffer & tf_buffer,   const std::string global_frame = "map",  
           const std::string robot_frame = "base_link",   const double transform_timeout = 0.1,  
           const rclcpp::Time stamp = rclcpp::Time(   ) );
          
          /**
          * @brief get an arbitrary pose in a target frame
          * @param input_pose Pose to transform
          * @param transformed_pose Output transformation
          * @param tf_buffer TF buffer to use for the transformation
          * @param target_frame Frame to transform into
          * @param transform_timeout TF Timeout to use for transformation
          * @return bool Whether it could be transformed successfully
          */
      53  bool transformPoseInTargetFrame(  
           const geometry_msgs::msg::PoseStamped & input_pose,  
           geometry_msgs::msg::PoseStamped & transformed_pose,  
           tf2_ros::Buffer & tf_buffer,   const std::string target_frame,  
           const double transform_timeout = 0.1 );
          
          } // end namespace nav2_util
          
          #endif // NAV2_UTIL__ROBOT_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/service_client.hpp

          // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__SERVICE_CLIENT_HPP_
          #define NAV2_UTIL__SERVICE_CLIENT_HPP_
          
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          
          namespace nav2_util
          {
          
          /**
           * @class nav2_util::ServiceClient
           * @brief A simple wrapper on ROS2 services for invoke(   ) and block-style calling
           */
          template<class ServiceT>
      30  class ServiceClient
          {
          public:
           /**
           * @brief A constructor
           * @param service_name name of the service to call
           * @param provided_node Node to create the service client off of
           */
      38   explicit ServiceClient(  
      39   const std::string & service_name,  
      40   const rclcpp::Node::SharedPtr & provided_node )
           : service_name_(  service_name ),   node_(  provided_node )
           {
           callback_group_ = node_->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   node_->get_node_base_interface(   ) );
           client_ = node_->create_client<ServiceT>(  
           service_name,  
           rmw_qos_profile_services_default,  
           callback_group_ );
           }
          
           using RequestType = typename ServiceT::Request;
           using ResponseType = typename ServiceT::Response;
          
           /**
           * @brief Invoke the service and block until completed or timed out
           * @param request The request object to call the service using
           * @param timeout Maximum timeout to wait for,   default infinite
           * @return Response A pointer to the service response from the request
           */
           typename ResponseType::SharedPtr invoke(  
           typename RequestType::SharedPtr & request,  
           const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(  -1 ) )
           {
           while (  !client_->wait_for_service(  std::chrono::seconds(  1 ) ) ) {
           if (  !rclcpp::ok(   ) ) {
           throw std::runtime_error(  
           service_name_ + " service client: interrupted while waiting for service" );
           }
           RCLCPP_INFO(  
      72   node_->get_logger(   ),   "%s service client: waiting for service to appear...",  
           service_name_.c_str(   ) );
           }
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "%s service client: send async request",  
           service_name_.c_str(   ) );
           auto future_result = client_->async_send_request(  request );
          
           if (  callback_group_executor_.spin_until_future_complete(  future_result,   timeout ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           throw std::runtime_error(  service_name_ + " service client: async_send_request failed" );
           }
          
           return future_result.get(   );
           }
          
           /**
           * @brief Invoke the service and block until completed
           * @param request The request object to call the service using
           * @param Response A pointer to the service response from the request
           * @return bool Whether it was successfully called
           */
           bool invoke(  
           typename RequestType::SharedPtr & request,  
           typename ResponseType::SharedPtr & response )
           {
           while (  !client_->wait_for_service(  std::chrono::seconds(  1 ) ) ) {
           if (  !rclcpp::ok(   ) ) {
           throw std::runtime_error(  
           service_name_ + " service client: interrupted while waiting for service" );
           }
           RCLCPP_INFO(  
           node_->get_logger(   ),   "%s service client: waiting for service to appear...",  
           service_name_.c_str(   ) );
           }
          
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "%s service client: send async request",  
           service_name_.c_str(   ) );
           auto future_result = client_->async_send_request(  request );
          
           if (  callback_group_executor_.spin_until_future_complete(  future_result ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           return false;
           }
          
           response = future_result.get(   );
           return response.get(   );
           }
          
           /**
           * @brief Block until a service is available or timeout
           * @param timeout Maximum timeout to wait for,   default infinite
           * @return bool true if service is available
           */
           bool wait_for_service(  const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max(   ) )
           {
           return client_->wait_for_service(  timeout );
           }
          
          protected:
           std::string service_name_;
           rclcpp::Node::SharedPtr node_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
           typename rclcpp::Client<ServiceT>::SharedPtr client_;
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/simple_action_server.hpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
          #define NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
          
          #include <memory>
          #include <mutex>
          #include <string>
          #include <thread>
          #include <future>
          #include <chrono>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          #include "nav2_util/node_thread.hpp"
          
          namespace nav2_util
          {
          
          /**
           * @class nav2_util::SimpleActionServer
           * @brief An action server wrapper to make applications simpler using Actions
           */
          template<typename ActionT>
      37  class SimpleActionServer
          {
          public:
           // Callback function to complete main work. This should itself deal with its
           // own exceptions,   but if for some reason one is thrown,   it will be caught
           // in SimpleActionServer and terminate the action itself.
           typedef std::function<void (   )> ExecuteCallback;
          
           // Callback function to notify the user that an exception was thrown that
           // the simple action server caught (  or another failure ) and the action was
           // terminated. To avoid using,   catch exceptions in your application such that
           // the SimpleActionServer will never need to terminate based on failed action
           // ExecuteCallback.
           typedef std::function<void (   )> CompletionCallback;
          
           /**
           * @brief An constructor for SimpleActionServer
           * @param node Ptr to node to make actions
           * @param action_name Name of the action to call
           * @param execute_callback Execution callback function of Action
           * @param server_timeout Timeout to to react to stop or preemption requests
           * @param spin_thread Whether to spin with a dedicated thread internally
           * @param options Options to pass to the underlying rcl_action_server_t
           */
           template<typename NodeT>
           explicit SimpleActionServer(  
           NodeT node,  
           const std::string & action_name,  
           ExecuteCallback execute_callback,  
           CompletionCallback completion_callback = nullptr,  
           std::chrono::milliseconds server_timeout = std::chrono::milliseconds(  500 ),  
           bool spin_thread = false,  
           const rcl_action_server_options_t & options = rcl_action_server_get_default_options(   ) )
           : SimpleActionServer(  
           node->get_node_base_interface(   ),  
           node->get_node_clock_interface(   ),  
           node->get_node_logging_interface(   ),  
           node->get_node_waitables_interface(   ),  
           action_name,   execute_callback,   completion_callback,   server_timeout,   spin_thread,   options )
           {}
          
           /**
           * @brief An constructor for SimpleActionServer
           * @param <node interfaces> Abstract node interfaces to make actions
           * @param action_name Name of the action to call
           * @param execute_callback Execution callback function of Action
           * @param server_timeout Timeout to to react to stop or preemption requests
           * @param spin_thread Whether to spin with a dedicated thread internally
           * @param options Options to pass to the underlying rcl_action_server_t
           */
      87   explicit SimpleActionServer(  
           rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,  
           rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,  
           rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,  
           rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,  
           const std::string & action_name,  
           ExecuteCallback execute_callback,  
           CompletionCallback completion_callback = nullptr,  
           std::chrono::milliseconds server_timeout = std::chrono::milliseconds(  500 ),  
           bool spin_thread = false,  
           const rcl_action_server_options_t & options = rcl_action_server_get_default_options(   ) )
           : node_base_interface_(  node_base_interface ),  
           node_clock_interface_(  node_clock_interface ),  
           node_logging_interface_(  node_logging_interface ),  
           node_waitables_interface_(  node_waitables_interface ),  
           action_name_(  action_name ),  
           execute_callback_(  execute_callback ),  
           completion_callback_(  completion_callback ),  
           server_timeout_(  server_timeout ),  
           spin_thread_(  spin_thread )
           {
           using namespace std::placeholders; // NOLINT
           if (  spin_thread_ ) {
           callback_group_ = node_base_interface->create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,   false );
           }
           action_server_ = rclcpp_action::create_server<ActionT>(  
           node_base_interface_,  
           node_clock_interface_,  
           node_logging_interface_,  
           node_waitables_interface_,  
           action_name_,  
           std::bind(  &SimpleActionServer::handle_goal,   this,   _1,   _2 ),  
           std::bind(  &SimpleActionServer::handle_cancel,   this,   _1 ),  
           std::bind(  &SimpleActionServer::handle_accepted,   this,   _1 ),  
           options,  
           callback_group_ );
           if (  spin_thread_ ) {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           executor_->add_callback_group(  callback_group_,   node_base_interface_ );
           executor_thread_ = std::make_unique<nav2_util::NodeThread>(  executor_ );
           }
           }
          
           /**
           * @brief handle the goal requested: accept or reject. This implementation always accepts.
           * @param uuid Goal ID
           * @param Goal A shared pointer to the specific goal
           * @return GoalResponse response of the goal processed
           */
     137   rclcpp_action::GoalResponse handle_goal(  
           const rclcpp_action::GoalUUID & /*uuid*/,  
           std::shared_ptr<const typename ActionT::Goal>/*goal*/ )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !server_active_ ) {
           return rclcpp_action::GoalResponse::REJECT;
           }
          
           debug_msg(  "Received request for goal acceptance" );
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
           }
          
           /**
           * @brief Accepts cancellation requests of action server.
           * @param uuid Goal ID
           * @param Goal A server goal handle to cancel
           * @return CancelResponse response of the goal cancelled
           */
     157   rclcpp_action::CancelResponse handle_cancel(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !handle->is_active(   ) ) {
           warn_msg(  
           "Received request for goal cancellation,  "
           "but the handle is inactive,   so reject the request" );
           return rclcpp_action::CancelResponse::REJECT;
           }
          
           debug_msg(  "Received request for goal cancellation" );
           return rclcpp_action::CancelResponse::ACCEPT;
           }
          
           /**
           * @brief Handles accepted goals and adds to preempted queue to switch to
           * @param Goal A server goal handle to cancel
           */
     177   void handle_accepted(  const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           debug_msg(  "Receiving a new goal" );
          
           if (  is_active(  current_handle_ ) || is_running(   ) ) {
           debug_msg(  "An older goal is active,   moving the new goal to a pending slot." );
          
           if (  is_active(  pending_handle_ ) ) {
           debug_msg(  
           "The pending slot is occupied."
           " The previous pending goal will be terminated and replaced." );
           terminate(  pending_handle_ );
           }
           pending_handle_ = handle;
           preempt_requested_ = true;
           } else {
           if (  is_active(  pending_handle_ ) ) {
           // Shouldn't reach a state with a pending goal but no current one.
           error_msg(  "Forgot to handle a preemption. Terminating the pending goal." );
           terminate(  pending_handle_ );
           preempt_requested_ = false;
           }
          
           current_handle_ = handle;
          
           // Return quickly to avoid blocking the executor,   so spin up a new thread
           debug_msg(  "Executing goal asynchronously." );
           execution_future_ = std::async(  std::launch::async,   [this](   ) {work(   );} );
           }
           }
          
           /**
           * @brief Computed background work and processes stop requests
           */
     212   void work(   )
           {
           while (  rclcpp::ok(   ) && !stop_execution_ && is_active(  current_handle_ ) ) {
           debug_msg(  "Executing the goal..." );
           try {
           execute_callback_(   );
           } catch (  std::exception & ex ) {
           RCLCPP_ERROR(  
           node_logging_interface_->get_logger(   ),  
           "Action server failed while executing action callback: \"%s\"",   ex.what(   ) );
           terminate_all(   );
           completion_callback_(   );
           return;
           }
          
           debug_msg(  "Blocking processing of new goal handles." );
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  stop_execution_ ) {
           warn_msg(  "Stopping the thread per request." );
           terminate_all(   );
           completion_callback_(   );
           break;
           }
          
           if (  is_active(  current_handle_ ) ) {
           warn_msg(  "Current goal was not completed successfully." );
           terminate(  current_handle_ );
           completion_callback_(   );
           }
          
           if (  is_active(  pending_handle_ ) ) {
           debug_msg(  "Executing a pending handle on the existing thread." );
           accept_pending_goal(   );
           } else {
           debug_msg(  "Done processing available goals." );
           break;
           }
           }
           debug_msg(  "Worker thread done." );
           }
          
           /**
           * @brief Active action server
           */
     257   void activate(   )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           server_active_ = true;
           stop_execution_ = false;
           }
          
           /**
           * @brief Deactive action server
           */
     267   void deactivate(   )
           {
           debug_msg(  "Deactivating..." );
          
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           server_active_ = false;
           stop_execution_ = true;
           }
          
           if (  !execution_future_.valid(   ) ) {
           return;
           }
          
           if (  is_running(   ) ) {
           warn_msg(  
           "Requested to deactivate server but goal is still executing."
           " Should check if action server is running before deactivating." );
           }
          
           using namespace std::chrono; //NOLINT
           auto start_time = steady_clock::now(   );
           while (  execution_future_.wait_for(  milliseconds(  100 ) ) != std::future_status::ready ) {
           info_msg(  "Waiting for async process to finish." );
           if (  steady_clock::now(   ) - start_time >= server_timeout_ ) {
           terminate_all(   );
           completion_callback_(   );
           throw std::runtime_error(  "Action callback is still running and missed deadline to stop" );
           }
           }
          
           debug_msg(  "Deactivation completed." );
           }
          
           /**
           * @brief Whether the action server is munching on a goal
           * @return bool If its running or not
           */
     305   bool is_running(   )
           {
           return execution_future_.valid(   ) &&
           (  execution_future_.wait_for(  std::chrono::milliseconds(  0 ) ) ==
           std::future_status::timeout );
           }
          
           /**
           * @brief Whether the action server is active or not
           * @return bool If its active or not
           */
     316   bool is_server_active(   )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           return server_active_;
           }
          
           /**
           * @brief Whether the action server has been asked to be preempted with a new goal
           * @return bool If there's a preemption request or not
           */
     326   bool is_preempt_requested(   ) const
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           return preempt_requested_;
           }
          
           /**
           * @brief Accept pending goals
           * @return Goal Ptr to the goal that's going to be accepted
           */
     336   const std::shared_ptr<const typename ActionT::Goal> accept_pending_goal(   )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !pending_handle_ || !pending_handle_->is_active(   ) ) {
           error_msg(  "Attempting to get pending goal when not available" );
           return std::shared_ptr<const typename ActionT::Goal>(   );
           }
          
           if (  is_active(  current_handle_ ) && current_handle_ != pending_handle_ ) {
           debug_msg(  "Cancelling the previous goal" );
           current_handle_->abort(  empty_result(   ) );
           }
          
           current_handle_ = pending_handle_;
           pending_handle_.reset(   );
           preempt_requested_ = false;
          
           debug_msg(  "Preempted goal" );
          
           return current_handle_->get_goal(   );
           }
          
           /**
           * @brief Terminate pending goals
           */
     362   void terminate_pending_goal(   )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !pending_handle_ || !pending_handle_->is_active(   ) ) {
           error_msg(  "Attempting to terminate pending goal when not available" );
           return;
           }
          
           terminate(  pending_handle_ );
           preempt_requested_ = false;
          
           debug_msg(  "Pending goal terminated" );
           }
          
           /**
           * @brief Get the current goal object
           * @return Goal Ptr to the goal that's being processed currently
           */
     381   const std::shared_ptr<const typename ActionT::Goal> get_current_goal(   ) const
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !is_active(  current_handle_ ) ) {
           error_msg(  "A goal is not available or has reached a final state" );
           return std::shared_ptr<const typename ActionT::Goal>(   );
           }
          
           return current_handle_->get_goal(   );
           }
          
           /**
           * @brief Get the pending goal object
           * @return Goal Ptr to the goal that's pending
           */
     397   const std::shared_ptr<const typename ActionT::Goal> get_pending_goal(   ) const
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  !pending_handle_ || !pending_handle_->is_active(   ) ) {
           error_msg(  "Attempting to get pending goal when not available" );
           return std::shared_ptr<const typename ActionT::Goal>(   );
           }
          
           return pending_handle_->get_goal(   );
           }
          
           /**
           * @brief Whether or not a cancel command has come in
           * @return bool Whether a cancel command has been requested or not
           */
     413   bool is_cancel_requested(   ) const
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           // A cancel request is assumed if either handle is canceled by the client.
          
           if (  current_handle_ == nullptr ) {
           error_msg(  "Checking for cancel but current goal is not available" );
           return false;
           }
          
           if (  pending_handle_ != nullptr ) {
           return pending_handle_->is_canceling(   );
           }
          
           return current_handle_->is_canceling(   );
           }
          
           /**
           * @brief Terminate all pending and active actions
           * @param result A result object to send to the terminated actions
           */
     435   void terminate_all(  
           typename std::shared_ptr<typename ActionT::Result> result =
           std::make_shared<typename ActionT::Result>(   ) )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           terminate(  current_handle_,   result );
           terminate(  pending_handle_,   result );
           preempt_requested_ = false;
           }
          
           /**
           * @brief Terminate the active action
           * @param result A result object to send to the terminated action
           */
     449   void terminate_current(  
           typename std::shared_ptr<typename ActionT::Result> result =
           std::make_shared<typename ActionT::Result>(   ) )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
           terminate(  current_handle_,   result );
           }
          
           /**
           * @brief Return success of the active action
           * @param result A result object to send to the terminated actions
           */
     461   void succeeded_current(  
           typename std::shared_ptr<typename ActionT::Result> result =
           std::make_shared<typename ActionT::Result>(   ) )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  is_active(  current_handle_ ) ) {
           debug_msg(  "Setting succeed on current goal." );
           current_handle_->succeed(  result );
           current_handle_.reset(   );
           }
           }
          
           /**
           * @brief Publish feedback to the action server clients
           * @param feedback A feedback object to send to the clients
           */
     478   void publish_feedback(  typename std::shared_ptr<typename ActionT::Feedback> feedback )
           {
           if (  !is_active(  current_handle_ ) ) {
           error_msg(  "Trying to publish feedback when the current goal handle is not active" );
           return;
           }
          
           current_handle_->publish_feedback(  feedback );
           }
          
          protected:
           // The SimpleActionServer isn't itself a node,   so it needs interfaces to one
           rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
           rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface_;
           rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;
           rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface_;
           std::string action_name_;
          
           ExecuteCallback execute_callback_;
           CompletionCallback completion_callback_;
           std::future<void> execution_future_;
           bool stop_execution_{false};
          
           mutable std::recursive_mutex update_mutex_;
           bool server_active_{false};
           bool preempt_requested_{false};
           std::chrono::milliseconds server_timeout_;
          
           std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> current_handle_;
           std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> pending_handle_;
          
           typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
           bool spin_thread_;
           rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
           rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
           std::unique_ptr<nav2_util::NodeThread> executor_thread_;
          
           /**
           * @brief Generate an empty result object for an action type
           */
           constexpr auto empty_result(   ) const
           {
           return std::make_shared<typename ActionT::Result>(   );
           }
          
           /**
           * @brief Whether a given goal handle is currently active
           * @param handle Goal handle to check
           * @return Whether this goal handle is active
           */
           constexpr bool is_active(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle ) const
           {
           return handle != nullptr && handle->is_active(   );
           }
          
           /**
           * @brief Terminate a particular action with a result
           * @param handle goal handle to terminate
           * @param the Results object to terminate the action with
           */
           void terminate(  
           std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle,  
           typename std::shared_ptr<typename ActionT::Result> result =
           std::make_shared<typename ActionT::Result>(   ) )
           {
           std::lock_guard<std::recursive_mutex> lock(  update_mutex_ );
          
           if (  is_active(  handle ) ) {
           if (  handle->is_canceling(   ) ) {
           warn_msg(  "Client requested to cancel the goal. Cancelling." );
           handle->canceled(  result );
           } else {
           warn_msg(  "Aborting handle." );
           handle->abort(  result );
           }
           handle.reset(   );
           }
           }
          
           /**
           * @brief Info logging
           */
           void info_msg(  const std::string & msg ) const
           {
           RCLCPP_INFO(  
           node_logging_interface_->get_logger(   ),  
           "[%s] [ActionServer] %s",   action_name_.c_str(   ),   msg.c_str(   ) );
           }
          
           /**
           * @brief Debug logging
           */
           void debug_msg(  const std::string & msg ) const
           {
           RCLCPP_DEBUG(  
           node_logging_interface_->get_logger(   ),  
           "[%s] [ActionServer] %s",   action_name_.c_str(   ),   msg.c_str(   ) );
           }
          
           /**
           * @brief Error logging
           */
           void error_msg(  const std::string & msg ) const
           {
           RCLCPP_ERROR(  
           node_logging_interface_->get_logger(   ),  
           "[%s] [ActionServer] %s",   action_name_.c_str(   ),   msg.c_str(   ) );
           }
          
           /**
           * @brief Warn logging
           */
           void warn_msg(  const std::string & msg ) const
           {
           RCLCPP_WARN(  
           node_logging_interface_->get_logger(   ),  
           "[%s] [ActionServer] %s",   action_name_.c_str(   ),   msg.c_str(   ) );
           }
          };
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_

navigation2/nav2_util/include/nav2_util/string_utils.hpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__STRING_UTILS_HPP_
          #define NAV2_UTIL__STRING_UTILS_HPP_
          
          #include <string>
          #include <vector>
          
          namespace nav2_util
          {
          
          typedef std::vector<std::string> Tokens;
          
          /*
           * @brief Remove leading slash from a topic name
           * @param in String of topic in
           * @return String out without slash
          */
      31  std::string strip_leading_slash(  const std::string & in );
          
          ///
          /*
           * @brief Split a string at the delimiters
           * @param in String to split
           * @param Delimiter criteria
           * @return Tokens
          */
      40  Tokens split(  const std::string & tokenstring,   char delimiter );
          
          } // namespace nav2_util
          
          #endif // NAV2_UTIL__STRING_UTILS_HPP_

navigation2/nav2_util/src/costmap.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <algorithm>
          #include "nav2_util/costmap.hpp"
          #include "tf2/LinearMath/Quaternion.h"
          #include "nav2_util/geometry_utils.hpp"
          
          using std::vector;
          
          namespace nav2_util
          {
          using nav2_util::geometry_utils::orientationAroundZAxis;
          
          const Costmap::CostValue Costmap::no_information = 255;
          const Costmap::CostValue Costmap::lethal_obstacle = 254;
          const Costmap::CostValue Costmap::inscribed_inflated_obstacle = 253;
          const Costmap::CostValue Costmap::medium_cost = 128;
          const Costmap::CostValue Costmap::free_space = 0;
          
          // TODO(  orduno ): Port ROS1 Costmap package
      34  Costmap::Costmap(  
      35   rclcpp::Node * node,   bool trinary_costmap,   bool track_unknown_space,  
           int lethal_threshold,   int unknown_cost_value )
          : node_(  node ),   trinary_costmap_(  trinary_costmap ),   track_unknown_space_(  track_unknown_space ),  
           lethal_threshold_(  lethal_threshold ),   unknown_cost_value_(  unknown_cost_value )
          {
           if (  lethal_threshold_ < 0. || lethal_threshold_ > 100. ) {
           RCLCPP_WARN(  
           node_->get_logger(   ),   "Costmap: Lethal threshold set to %d,   it should be within"
           " bounds 0-100. This could result in potential collisions!",   lethal_threshold_ );
           // lethal_threshold_ = std::max(  std::min(  lethal_threshold_,   100 ),   0 );
           }
          }
          
      48  Costmap::~Costmap(   )
          {
          }
          
      52  void Costmap::set_static_map(  const nav_msgs::msg::OccupancyGrid & occupancy_grid )
          {
           RCLCPP_INFO(  node_->get_logger(   ),   "Costmap: Setting static costmap" );
          
           costmap_properties_.map_load_time = node_->now(   );
           costmap_properties_.update_time = node_->now(   );
           costmap_properties_.layer = "Master";
          
           // Store the properties of the occupancy grid
           costmap_properties_.resolution = occupancy_grid.info.resolution;
           costmap_properties_.size_x = occupancy_grid.info.width;
           costmap_properties_.size_y = occupancy_grid.info.height;
           costmap_properties_.origin = occupancy_grid.info.origin;
          
           uint32_t size_x = costmap_properties_.size_x;
           uint32_t size_y = costmap_properties_.size_y;
          
           costs_.resize(  size_x * size_y );
          
           // TODO(  orduno ): for now just doing a direct mapping of values from the original static map
           // i.e. no cell inflation,   etc.
           std::vector<int8_t> static_map_cell_values = occupancy_grid.data;
          
           unsigned int index = 0;
           for (  unsigned int i = 0; i < size_y; ++i ) {
           for (  unsigned int j = 0; j < size_x; ++j ) {
           unsigned char value = static_map_cell_values[index];
           costs_[index] = interpret_value(  value );
           ++index;
           }
           }
          
           map_provided_ = true;
          }
          
      87  void Costmap::set_test_costmap(  const TestCostmap & testCostmapType )
          {
           costmap_properties_.map_load_time = node_->now(   );
           costmap_properties_.update_time = node_->now(   );
           costmap_properties_.layer = "master";
           costmap_properties_.resolution = 1;
           costmap_properties_.size_x = 10;
           costmap_properties_.size_y = 10;
           costmap_properties_.origin.position.x = 0.0;
           costmap_properties_.origin.position.y = 0.0;
           costmap_properties_.origin.position.z = 0.0;
          
           // Define map rotation
           // Provided as yaw with counterclockwise rotation,   with yaw = 0 meaning no rotation
           costmap_properties_.origin.orientation = orientationAroundZAxis(  0.0 );
          
           costs_ = get_test_data(  testCostmapType );
          
           using_test_map_ = true;
          }
          
     108  nav2_msgs::msg::Costmap Costmap::get_costmap(  
     109   const nav2_msgs::msg::CostmapMetaData & /*specifications*/ )
          {
           if (  !map_provided_ && !using_test_map_ ) {
           throw std::runtime_error(  "Costmap has not been set." );
           }
          
           // TODO(  orduno ): build a costmap given the specifications
           // for now using the specs of the static map
          
           nav2_msgs::msg::Costmap costmap;
          
           costmap.header.stamp = node_->now(   );
           costmap.header.frame_id = "map";
           costmap.metadata = costmap_properties_;
           costmap.data = costs_;
          
           return costmap;
          }
          
     128  vector<uint8_t> Costmap::get_test_data(  const TestCostmap testCostmapType )
          {
           // TODO(  orduno ): alternatively use a mathematical function
          
           const uint8_t n = no_information;
           const uint8_t x = lethal_obstacle;
           const uint8_t i = inscribed_inflated_obstacle;
           const uint8_t u = medium_cost;
           const uint8_t o = free_space;
          
           vector<uint8_t> costmapFree =
           // 0 1 2 3 4 5 6 7 8 9
           {o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 0
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 1
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 2
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 3
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 4
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 5
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 6
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 7
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o,   // 8
           o,   o,   o,   o,   o,   o,   o,   o,   o,   o}; // 9
          
           vector<uint8_t> costmapBounded =
           // 0 1 2 3 4 5 6 7 8 9
           {n,   n,   n,   n,   n,   n,   n,   n,   n,   n,   // 0
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 1
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 2
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 3
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 4
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 5
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 6
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 7
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 8
           n,   n,   n,   n,   n,   n,   n,   n,   n,   n}; // 9
          
           vector<uint8_t> costmapObstacleBL =
           // 0 1 2 3 4 5 6 7 8 9
           {n,   n,   n,   n,   n,   n,   n,   n,   n,   n,   // 0
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 1
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 2
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 3
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 4
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 5
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 6
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 7
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 8
           n,   n,   n,   n,   n,   n,   n,   n,   n,   n}; // 9
          
           vector<uint8_t> costmapObstacleTL =
           // 0 1 2 3 4 5 6 7 8 9
           {n,   n,   n,   n,   n,   n,   n,   n,   n,   n,   // 0
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 1
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 2
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 3
           n,   o,   x,   x,   x,   o,   o,   o,   o,   n,   // 4
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 5
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 6
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 7
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 8
           n,   n,   n,   n,   n,   n,   n,   n,   n,   n}; // 9
          
           vector<uint8_t> costmapMaze =
           // 0 1 2 3 4 5 6 7 8 9
           {n,   n,   n,   n,   n,   n,   n,   n,   n,   n,   // 0
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 1
           n,   x,   x,   o,   x,   x,   x,   o,   x,   n,   // 2
           n,   o,   o,   o,   o,   x,   o,   o,   o,   n,   // 3
           n,   o,   x,   x,   o,   x,   o,   x,   o,   n,   // 4
           n,   o,   x,   x,   o,   x,   o,   x,   o,   n,   // 5
           n,   o,   o,   x,   o,   x,   o,   x,   o,   n,   // 6
           n,   x,   o,   x,   o,   x,   o,   x,   o,   n,   // 7
           n,   o,   o,   o,   o,   o,   o,   x,   o,   n,   // 8
           n,   n,   n,   n,   n,   n,   n,   n,   n,   n}; // 9
          
           vector<uint8_t> costmapMaze2 =
           // 0 1 2 3 4 5 6 7 8 9
           {n,   n,   n,   n,   n,   n,   n,   n,   n,   n,   // 0
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 1
           n,   x,   x,   u,   x,   x,   x,   o,   x,   n,   // 2
           n,   o,   o,   o,   o,   o,   o,   o,   u,   n,   // 3
           n,   o,   x,   x,   o,   x,   x,   x,   u,   n,   // 4
           n,   o,   x,   x,   o,   o,   o,   x,   u,   n,   // 5
           n,   o,   o,   x,   u,   x,   o,   x,   u,   n,   // 6
           n,   x,   o,   x,   u,   x,   i,   x,   u,   n,   // 7
           n,   o,   o,   o,   o,   o,   o,   o,   o,   n,   // 8
           n,   n,   n,   n,   n,   n,   n,   n,   n,   n}; // 9
          
           switch (  testCostmapType ) {
           case TestCostmap::open_space:
           return costmapFree;
           case TestCostmap::bounded:
           return costmapBounded;
           case TestCostmap::bottom_left_obstacle:
           return costmapObstacleBL;
           case TestCostmap::top_left_obstacle:
           return costmapObstacleTL;
           case TestCostmap::maze1:
           return costmapMaze;
           case TestCostmap::maze2:
           return costmapMaze2;
           default:
           return costmapFree;
           }
          }
          
     234  uint8_t Costmap::interpret_value(  const int8_t value ) const
          {
           if (  track_unknown_space_ && value == unknown_cost_value_ ) {
           return no_information;
           } else if (  !track_unknown_space_ && value == unknown_cost_value_ ) {
           return free_space;
           } else if (  value >= lethal_threshold_ ) {
           return lethal_obstacle;
           } else if (  trinary_costmap_ ) {
           return free_space;
           }
          
           double scale = static_cast<double>(  value / lethal_threshold_ );
           return static_cast<uint8_t>(  scale * lethal_obstacle );
          }
          
     250  bool Costmap::is_free(  const unsigned int x_coordinate,   const unsigned int y_coordinate ) const
          {
           unsigned int index = y_coordinate * costmap_properties_.size_x + x_coordinate;
          
           return is_free(  index );
          }
          
     257  bool Costmap::is_free(  const unsigned int index ) const
          {
           if (  costs_[index] < Costmap::inscribed_inflated_obstacle ) {
           return true;
           }
          
           return false;
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/lifecycle_bringup_commandline.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          #include <string>
          #include <iostream>
          #include <cstdlib>
          #include <chrono>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_utils.hpp"
          
          using std::cerr;
          using namespace std::chrono_literals;
          
      26  void usage(   )
          {
           cerr << "Invalid command line.\n\n";
           cerr << "This command will take a set of unconfigured lifecycle nodes through the\n";
           cerr << "CONFIGURED to the ACTIVATED state\n";
           cerr << "The nodes are brought up in the order listed on the command line\n\n";
           cerr << "Usage:\n";
           cerr << " > lifecycle_startup <node name> ...\n";
           std::exit(  1 );
          }
          
      37  int main(  int argc,   char * argv[] )
          {
           if (  argc == 1 ) {
           usage(   );
           }
           rclcpp::init(  0,   nullptr );
           nav2_util::startup_lifecycle_nodes(  
           std::vector<std::string>(  argv + 1,   argv + argc ),  
           10s );
           rclcpp::shutdown(   );
          }

navigation2/nav2_util/src/lifecycle_node.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_util/lifecycle_node.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace nav2_util
          {
          
      26  LifecycleNode::LifecycleNode(  
      27   const std::string & node_name,  
      28   const std::string & ns,  
      29   const rclcpp::NodeOptions & options )
          : rclcpp_lifecycle::LifecycleNode(  node_name,   ns,   options )
          {
           // server side never times out from lifecycle manager
           this->declare_parameter(  bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM,   true );
           this->set_parameter(  
           rclcpp::Parameter(  
           bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM,   true ) );
          
           printLifecycleNodeNotification(   );
          }
          
      41  LifecycleNode::~LifecycleNode(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Destroying" );
           // In case this lifecycle node wasn't properly shut down,   do it here
           if (  get_current_state(   ).id(   ) ==
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           on_deactivate(  get_current_state(   ) );
           on_cleanup(  get_current_state(   ) );
           }
          }
          
      53  void LifecycleNode::createBond(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating bond (  %s ) to lifecycle manager.",   this->get_name(   ) );
          
           bond_ = std::make_unique<bond::Bond>(  
           std::string(  "bond" ),  
           this->get_name(   ),  
           shared_from_this(   ) );
          
           bond_->setHeartbeatPeriod(  0.10 );
           bond_->setHeartbeatTimeout(  4.0 );
           bond_->start(   );
          }
          
      67  void LifecycleNode::destroyBond(   )
          {
           RCLCPP_INFO(  get_logger(   ),   "Destroying bond (  %s ) to lifecycle manager.",   this->get_name(   ) );
          
           if (  bond_ ) {
           bond_.reset(   );
           }
          }
          
      76  void LifecycleNode::printLifecycleNodeNotification(   )
          {
           RCLCPP_INFO(  
           get_logger(   ),  
           "\n\t%s lifecycle node launched. \n"
           "\tWaiting on external lifecycle transitions to activate\n"
           "\tSee https://design.ros2.org/articles/node_lifecycle.html for more information.",   get_name(   ) );
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/lifecycle_service_client.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_util/lifecycle_service_client.hpp"
          
          #include <string>
          #include <chrono>
          #include <memory>
          
          #include "lifecycle_msgs/srv/change_state.hpp"
          #include "lifecycle_msgs/srv/get_state.hpp"
          
          using nav2_util::generate_internal_node;
          using std::chrono::seconds;
          using std::make_shared;
          using std::string;
          using namespace std::chrono_literals;
          
          namespace nav2_util
          {
          
      33  LifecycleServiceClient::LifecycleServiceClient(  const string & lifecycle_node_name )
          : node_(  generate_internal_node(  lifecycle_node_name + "_lifecycle_client" ) ),  
           change_state_(  lifecycle_node_name + "/change_state",   node_ ),  
           get_state_(  lifecycle_node_name + "/get_state",   node_ )
          {
           // Block until server is up
           get_state_.wait_for_service(   );
          }
          
      42  LifecycleServiceClient::LifecycleServiceClient(  
      43   const string & lifecycle_node_name,  
      44   rclcpp::Node::SharedPtr parent_node )
          : node_(  parent_node ),  
           change_state_(  lifecycle_node_name + "/change_state",   node_ ),  
           get_state_(  lifecycle_node_name + "/get_state",   node_ )
          {
           // Block until server is up
           get_state_.wait_for_service(   );
          }
          
      53  bool LifecycleServiceClient::change_state(  
      54   const uint8_t transition,  
      55   const seconds timeout )
          {
           if (  !change_state_.wait_for_service(  timeout ) ) {
           throw std::runtime_error(  "change_state service is not available!" );
           }
          
           auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>(   );
           request->transition.id = transition;
           auto response = change_state_.invoke(  request,   timeout );
           return response.get(   );
          }
          
      67  bool LifecycleServiceClient::change_state(  
      68   std::uint8_t transition )
          {
           if (  !change_state_.wait_for_service(  5s ) ) {
           throw std::runtime_error(  "change_state service is not available!" );
           }
          
           auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>(   );
           auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>(   );
           request->transition.id = transition;
           return change_state_.invoke(  request,   response );
          }
          
      80  uint8_t LifecycleServiceClient::get_state(  
      81   const seconds timeout )
          {
           if (  !get_state_.wait_for_service(  timeout ) ) {
           throw std::runtime_error(  "get_state service is not available!" );
           }
          
           auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>(   );
           auto result = get_state_.invoke(  request,   timeout );
           return result->current_state.id;
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/lifecycle_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <string>
          #include <thread>
          #include <vector>
          
          #include "lifecycle_msgs/srv/change_state.hpp"
          #include "lifecycle_msgs/srv/get_state.hpp"
          #include "nav2_util/lifecycle_service_client.hpp"
          
          using std::string;
          using lifecycle_msgs::msg::Transition;
          
          namespace nav2_util
          {
          
          #define RETRY(  fn,   retries ) \
           { \
      32   int count = 0; \
           while (  true ) { \
           try { \
           fn; \
           break; \
           } catch (  std::runtime_error & e ) { \
           ++count; \
           if (  count > (  retries ) ) { \
           throw e;} \
           } \
           } \
           }
          
          static void startupLifecycleNode(  
           const std::string & node_name,  
           const std::chrono::seconds service_call_timeout,  
           const int retries )
          {
      50   LifecycleServiceClient sc(  node_name );
          
           // Despite waiting for the service to be available and using reliable transport
           // service calls still frequently hang. To get reliable startup it's necessary
           // to timeout the service call and retry it when that happens.
           RETRY(  
           sc.change_state(  Transition::TRANSITION_CONFIGURE,   service_call_timeout ),  
           retries );
           RETRY(  
           sc.change_state(  Transition::TRANSITION_ACTIVATE,   service_call_timeout ),  
      60   retries );
          }
          
          void startup_lifecycle_nodes(  
           const std::vector<std::string> & node_names,  
           const std::chrono::seconds service_call_timeout,  
           const int retries )
          {
           for (  const auto & node_name : node_names ) {
           startupLifecycleNode(  node_name,   service_call_timeout,   retries );
           }
          }
          
          static void resetLifecycleNode(  
           const std::string & node_name,  
           const std::chrono::seconds service_call_timeout,  
           const int retries )
          {
      78   LifecycleServiceClient sc(  node_name );
          
           // Despite waiting for the service to be available and using reliable transport
           // service calls still frequently hang. To get reliable reset it's necessary
           // to timeout the service call and retry it when that happens.
           RETRY(  
           sc.change_state(  Transition::TRANSITION_DEACTIVATE,   service_call_timeout ),  
           retries );
           RETRY(  
           sc.change_state(  Transition::TRANSITION_CLEANUP,   service_call_timeout ),  
           retries );
          }
          
          void reset_lifecycle_nodes(  
           const std::vector<std::string> & node_names,  
           const std::chrono::seconds service_call_timeout,  
           const int retries )
          {
           for (  const auto & node_name : node_names ) {
           resetLifecycleNode(  node_name,   service_call_timeout,   retries );
           }
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/node_thread.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_util/node_thread.hpp"
          
          namespace nav2_util
          {
          
      22  NodeThread::NodeThread(  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base )
          : node_(  node_base )
          {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           thread_ = std::make_unique<std::thread>(  
           [&](   )
           {
           executor_->add_node(  node_ );
           executor_->spin(   );
           executor_->remove_node(  node_ );
           } );
          }
          
      35  NodeThread::NodeThread(  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor )
          : executor_(  executor )
          {
           thread_ = std::make_unique<std::thread>(  [&](   ) {executor_->spin(   );} );
          }
          
      41  NodeThread::~NodeThread(   )
          {
           executor_->cancel(   );
           thread_->join(   );
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/node_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_util/node_utils.hpp"
          #include <chrono>
          #include <string>
          #include <algorithm>
          #include <cctype>
          
          using std::chrono::high_resolution_clock;
          using std::to_string;
          using std::string;
          using std::replace_if;
          using std::isalnum;
          
          namespace nav2_util
          {
          
      30  string sanitize_node_name(  const string & potential_node_name )
          {
           string node_name(  potential_node_name );
           // read this as `replace` characters in `node_name` `if` not alphanumeric.
           // replace with '_'
           replace_if(  
           begin(  node_name ),   end(  node_name ),  
           [](  auto c ) {return !isalnum(  c );},  
           '_' );
           return node_name;
          }
          
      42  string add_namespaces(  const string & top_ns,   const string & sub_ns )
          {
           if (  !top_ns.empty(   ) && top_ns.back(   ) == '/' ) {
           if (  top_ns.front(   ) == '/' ) {
           return top_ns + sub_ns;
           } else {
           return "/" + top_ns + sub_ns;
           }
           }
          
           return top_ns + "/" + sub_ns;
          }
          
      55  std::string time_to_string(  size_t len )
          {
           string output(  len,   '0' ); // prefill the string with zeros
           auto timepoint = high_resolution_clock::now(   );
           auto timecount = timepoint.time_since_epoch(   ).count(   );
           auto timestring = to_string(  timecount );
           if (  timestring.length(   ) >= len ) {
           // if `timestring` is shorter,   put it at the end of `output`
           output.replace(  
           0,   len,  
           timestring,  
           timestring.length(   ) - len,   len );
           } else {
           // if `output` is shorter,   just copy in the end of `timestring`
           output.replace(  
           len - timestring.length(   ),   timestring.length(   ),  
           timestring,  
           0,   timestring.length(   ) );
           }
           return output;
          }
          
      77  std::string generate_internal_node_name(  const std::string & prefix )
          {
           return sanitize_node_name(  prefix ) + "_" + time_to_string(  8 );
          }
          
      82  rclcpp::Node::SharedPtr generate_internal_node(  const std::string & prefix )
          {
           auto options =
           rclcpp::NodeOptions(   )
           .start_parameter_services(  false )
           .start_parameter_event_publisher(  false )
           .arguments(  {"--ros-args",   "-r",   "__node:=" + generate_internal_node_name(  prefix ),   "--"} );
           return rclcpp::Node::make_shared(  "_",   options );
          }
          
          rclcpp::NodeOptions
      93  get_node_options_default(  bool allow_undeclared,   bool declare_initial_params )
          {
           rclcpp::NodeOptions options;
           options.allow_undeclared_parameters(  allow_undeclared );
           options.automatically_declare_parameters_from_overrides(  declare_initial_params );
           return options;
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/odometry_utils.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "nav2_util/odometry_utils.hpp"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
          namespace nav2_util
          {
          
      26  OdomSmoother::OdomSmoother(  
      27   const rclcpp::Node::WeakPtr & parent,  
           double filter_duration,  
      29   const std::string & odom_topic )
          : odom_history_duration_(  rclcpp::Duration::from_seconds(  filter_duration ) )
          {
           auto node = parent.lock(   );
           odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(  
           odom_topic,  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &OdomSmoother::odomCallback,   this,   std::placeholders::_1 ) );
          
           odom_cumulate_.twist.twist.linear.x = 0;
           odom_cumulate_.twist.twist.linear.y = 0;
           odom_cumulate_.twist.twist.linear.z = 0;
           odom_cumulate_.twist.twist.angular.x = 0;
           odom_cumulate_.twist.twist.angular.y = 0;
           odom_cumulate_.twist.twist.angular.z = 0;
          }
          
      46  OdomSmoother::OdomSmoother(  
      47   const nav2_util::LifecycleNode::WeakPtr & parent,  
           double filter_duration,  
      49   const std::string & odom_topic )
          : odom_history_duration_(  rclcpp::Duration::from_seconds(  filter_duration ) )
          {
           auto node = parent.lock(   );
           odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(  
           odom_topic,  
           rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &OdomSmoother::odomCallback,   this,   std::placeholders::_1 ) );
          
           odom_cumulate_.twist.twist.linear.x = 0;
           odom_cumulate_.twist.twist.linear.y = 0;
           odom_cumulate_.twist.twist.linear.z = 0;
           odom_cumulate_.twist.twist.angular.x = 0;
           odom_cumulate_.twist.twist.angular.y = 0;
           odom_cumulate_.twist.twist.angular.z = 0;
          }
          
      66  void OdomSmoother::odomCallback(  const nav_msgs::msg::Odometry::SharedPtr msg )
          {
           std::lock_guard<std::mutex> lock(  odom_mutex_ );
          
           // update cumulated odom only if history is not empty
           if (  !odom_history_.empty(   ) ) {
           // to store current time
           auto current_time = rclcpp::Time(  msg->header.stamp );
          
           // to store time of the first odom in history
           auto front_time = rclcpp::Time(  odom_history_.front(   ).header.stamp );
          
           // update cumulated odom when duration has exceeded and pop earliest msg
           while (  current_time - front_time > odom_history_duration_ ) {
           const auto & odom = odom_history_.front(   );
           odom_cumulate_.twist.twist.linear.x -= odom.twist.twist.linear.x;
           odom_cumulate_.twist.twist.linear.y -= odom.twist.twist.linear.y;
           odom_cumulate_.twist.twist.linear.z -= odom.twist.twist.linear.z;
           odom_cumulate_.twist.twist.angular.x -= odom.twist.twist.angular.x;
           odom_cumulate_.twist.twist.angular.y -= odom.twist.twist.angular.y;
           odom_cumulate_.twist.twist.angular.z -= odom.twist.twist.angular.z;
           odom_history_.pop_front(   );
          
           if (  odom_history_.empty(   ) ) {
           break;
           }
          
           // update with the timestamp of earliest odom message in history
           front_time = rclcpp::Time(  odom_history_.front(   ).header.stamp );
           }
           }
          
           odom_history_.push_back(  *msg );
           updateState(   );
          }
          
     102  void OdomSmoother::updateState(   )
          {
           const auto & odom = odom_history_.back(   );
           odom_cumulate_.twist.twist.linear.x += odom.twist.twist.linear.x;
           odom_cumulate_.twist.twist.linear.y += odom.twist.twist.linear.y;
           odom_cumulate_.twist.twist.linear.z += odom.twist.twist.linear.z;
           odom_cumulate_.twist.twist.angular.x += odom.twist.twist.angular.x;
           odom_cumulate_.twist.twist.angular.y += odom.twist.twist.angular.y;
           odom_cumulate_.twist.twist.angular.z += odom.twist.twist.angular.z;
          
           vel_smooth_.header = odom.header;
           vel_smooth_.twist.linear.x = odom_cumulate_.twist.twist.linear.x / odom_history_.size(   );
           vel_smooth_.twist.linear.y = odom_cumulate_.twist.twist.linear.y / odom_history_.size(   );
           vel_smooth_.twist.linear.z = odom_cumulate_.twist.twist.linear.z / odom_history_.size(   );
           vel_smooth_.twist.angular.x = odom_cumulate_.twist.twist.angular.x / odom_history_.size(   );
           vel_smooth_.twist.angular.y = odom_cumulate_.twist.twist.angular.y / odom_history_.size(   );
           vel_smooth_.twist.angular.z = odom_cumulate_.twist.twist.angular.z / odom_history_.size(   );
          }
          
          } // namespace nav2_util

navigation2/nav2_util/src/robot_utils.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2019 Steven Macenski
          // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          #include <memory>
          
          #include "nav2_util/robot_utils.hpp"
          #include "rclcpp/logger.hpp"
          
          namespace nav2_util
          {
          
      26  bool getCurrentPose(  
           geometry_msgs::msg::PoseStamped & global_pose,  
           tf2_ros::Buffer & tf_buffer,   const std::string global_frame,  
           const std::string robot_frame,   const double transform_timeout,  
           const rclcpp::Time stamp )
          {
           tf2::toMsg(  tf2::Transform::getIdentity(   ),   global_pose.pose );
           global_pose.header.frame_id = robot_frame;
           global_pose.header.stamp = stamp;
          
           return transformPoseInTargetFrame(  
           global_pose,   global_pose,   tf_buffer,   global_frame,   transform_timeout );
          }
          
      40  bool transformPoseInTargetFrame(  
           const geometry_msgs::msg::PoseStamped & input_pose,  
           geometry_msgs::msg::PoseStamped & transformed_pose,  
           tf2_ros::Buffer & tf_buffer,   const std::string target_frame,  
           const double transform_timeout )
          {
           static rclcpp::Logger logger = rclcpp::get_logger(  "transformPoseInTargetFrame" );
          
           try {
           transformed_pose = tf_buffer.transform(  
           input_pose,   target_frame,  
           tf2::durationFromSec(  transform_timeout ) );
           return true;
           } catch (  tf2::LookupException & ex ) {
           RCLCPP_ERROR(  
           logger,  
           "No Transform available Error looking up target frame: %s\n",   ex.what(   ) );
           } catch (  tf2::ConnectivityException & ex ) {
           RCLCPP_ERROR(  
           logger,  
           "Connectivity Error looking up target frame: %s\n",   ex.what(   ) );
           } catch (  tf2::ExtrapolationException & ex ) {
           RCLCPP_ERROR(  
           logger,  
           "Extrapolation Error looking up target frame: %s\n",   ex.what(   ) );
           } catch (  tf2::TimeoutException & ex ) {
           RCLCPP_ERROR(  
           logger,  
           "Transform timeout with tolerance: %.4f",   transform_timeout );
           } catch (  tf2::TransformException & ex ) {
           RCLCPP_ERROR(  
           logger,   "Failed to transform from %s to %s",  
           input_pose.header.frame_id.c_str(   ),   target_frame.c_str(   ) );
           }
          
           return false;
          }
          
          } // end namespace nav2_util

navigation2/nav2_util/src/string_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_util/string_utils.hpp"
          #include <string>
          
          using std::string;
          
          namespace nav2_util
          {
          
      23  std::string strip_leading_slash(  const string & in )
          {
           string out = in;
          
           if (  (  !in.empty(   ) ) && (  in[0] == '/' ) ) {
           out.erase(  0,   1 );
           }
          
           return out;
          }
          
      34  Tokens split(  const string & tokenstring,   char delimiter )
          {
           Tokens tokens;
          
           size_t current_pos = 0;
           size_t pos = 0;
           while (  (  pos = tokenstring.find(  delimiter,   current_pos ) ) != string::npos ) {
           tokens.push_back(  tokenstring.substr(  current_pos,   pos - current_pos ) );
           current_pos = pos + 1;
           }
           tokens.push_back(  tokenstring.substr(  current_pos ) );
           return tokens;
          }
          
          } // namespace nav2_util

navigation2/nav2_util/test/test_actions.cpp

          // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <memory>
          #include <thread>
          
          #include "gtest/gtest.h"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "test_msgs/action/fibonacci.hpp"
          #include "std_msgs/msg/empty.hpp"
          
          using Fibonacci = test_msgs::action::Fibonacci;
          using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
          
          using std::placeholders::_1;
          using namespace std::chrono_literals;
          
      31  class FibonacciServerNode : public rclcpp::Node
          {
          public:
      34   FibonacciServerNode(   )
           : rclcpp::Node(  "fibonacci_server_node" )
           {
           }
          
      39   ~FibonacciServerNode(   )
           {
           }
          
      43   void on_init(   )
           {
           action_server_ = std::make_shared<nav2_util::SimpleActionServer<Fibonacci>>(  
           shared_from_this(   ),  
           "fibonacci",  
           std::bind(  &FibonacciServerNode::execute,   this ) );
          
           deactivate_subs_ = create_subscription<std_msgs::msg::Empty>(  
           "deactivate_server",  
           1,  
           [this](  std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
           RCLCPP_INFO(  this->get_logger(   ),   "Deactivating" );
           action_server_->deactivate(   );
           } );
          
           activate_subs_ = create_subscription<std_msgs::msg::Empty>(  
           "activate_server",  
           1,  
           [this](  std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
           RCLCPP_INFO(  this->get_logger(   ),   "Activating" );
           action_server_->activate(   );
           } );
          
           omit_preempt_subs_ = create_subscription<std_msgs::msg::Empty>(  
           "omit_preemption",  
           1,  
           [this](  std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
           RCLCPP_INFO(  this->get_logger(   ),   "Ignoring preemptions" );
           do_premptions_ = false;
           } );
           }
          
      75   void on_term(   )
           {
           // when nothing's running make sure everything's dead.
           const std::shared_ptr<const Fibonacci::Goal> a = action_server_->accept_pending_goal(   );
           const std::shared_ptr<const Fibonacci::Goal> b = action_server_->get_current_goal(   );
           assert(  a == b );
           assert(  action_server_->is_cancel_requested(   ) == false );
           auto feedback = std::make_shared<Fibonacci::Feedback>(   );
           action_server_->publish_feedback(  feedback );
           action_server_.reset(   );
           }
          
      87   void execute(   )
           {
           rclcpp::Rate loop_rate(  10 );
          
          preempted:
           // Initialize the goal,   feedback,   and result
           auto goal = action_server_->get_current_goal(   );
           auto feedback = std::make_shared<Fibonacci::Feedback>(   );
           auto result = std::make_shared<Fibonacci::Result>(   );
          
           // Fibonacci-specific initialization
           auto & sequence = feedback->sequence;
           sequence.push_back(  0 );
           sequence.push_back(  1 );
          
           for (  int i = 1; (  i < goal->order ) && rclcpp::ok(   ); ++i ) {
           // Should be check periodically if this action has been canceled
           // or if the server has been deactivated.
           if (  action_server_->is_cancel_requested(   ) || !action_server_->is_server_active(   ) ) {
           result->sequence = sequence;
           return;
           }
          
           // Check if we've gotten an new goal,   pre-empting the current one
           if (  do_premptions_ && action_server_->is_preempt_requested(   ) ) {
           action_server_->accept_pending_goal(   );
           goto preempted;
           }
          
           // Update the sequence
           sequence.push_back(  sequence[i] + sequence[i - 1] );
          
           // Publish feedback
           action_server_->publish_feedback(  feedback );
           loop_rate.sleep(   );
           }
          
           // Check if goal is done
           if (  rclcpp::ok(   ) ) {
           result->sequence = sequence;
           action_server_->succeeded_current(  result );
           }
           }
          
          private:
           std::shared_ptr<nav2_util::SimpleActionServer<Fibonacci>> action_server_;
           rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr deactivate_subs_;
           rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr activate_subs_;
           rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr omit_preempt_subs_;
          
           bool do_premptions_{true};
          };
          
          class RclCppFixture
          {
          public:
           RclCppFixture(   )
           {
           }
          
           void Setup(   )
           {
           server_thread_ =
           std::make_shared<std::thread>(  std::bind(  &RclCppFixture::server_thread_func,   this ) );
           }
          
           ~RclCppFixture(   )
           {
           server_thread_->join(   );
           }
          
           void server_thread_func(   )
           {
           auto node = std::make_shared<FibonacciServerNode>(   );
           node->on_init(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           node->on_term(   );
           node.reset(   );
           }
          
           std::shared_ptr<std::thread> server_thread_;
          };
          
          RclCppFixture g_rclcppfixture;
          
          class ActionTestNode : public rclcpp::Node
          {
          public:
           ActionTestNode(   )
           : rclcpp::Node(  nav2_util::generate_internal_node_name(  "action_test_node" ) )
           {
           }
          
           void on_init(   )
           {
           action_client_ = rclcpp_action::create_client<Fibonacci>(  shared_from_this(   ),   "fibonacci" );
           action_client_->wait_for_action_server(   );
          
           deactivate_pub_ = this->create_publisher<std_msgs::msg::Empty>(  "deactivate_server",   1 );
           activate_pub_ = this->create_publisher<std_msgs::msg::Empty>(  "activate_server",   1 );
           omit_prempt_pub_ = this->create_publisher<std_msgs::msg::Empty>(  "omit_preemption",   1 );
           }
          
           void on_term(   )
           {
           action_client_.reset(   );
           }
          
           void deactivate_server(   )
           {
           deactivate_pub_->publish(  std_msgs::msg::Empty(   ) );
           }
          
           void activate_server(   )
           {
           activate_pub_->publish(  std_msgs::msg::Empty(   ) );
           }
          
           void omit_server_preemptions(   )
           {
           omit_prempt_pub_->publish(  std_msgs::msg::Empty(   ) );
           }
          
           rclcpp_action::Client<Fibonacci>::SharedPtr action_client_;
           rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr deactivate_pub_;
           rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr activate_pub_;
           rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr omit_prempt_pub_;
          };
          
          class ActionTest : public ::testing::Test
          {
          protected:
           void SetUp(   ) override
           {
           node_ = std::make_shared<ActionTestNode>(   );
           node_->on_init(   );
           }
          
           void TearDown(   ) override
           {
           std::cout << " Teardown" << std::endl;
           node_->on_term(   );
           std::cout << " Teardown..." << std::endl;
           node_.reset(   );
           std::cout << " Teardown complete" << std::endl;
           }
          
           std::shared_ptr<ActionTestNode> node_;
          };
          
          TEST_F(  ActionTest,   test_simple_action )
          {
           node_->activate_server(   );
          
           // The goal for this invocation
           auto goal = Fibonacci::Goal(   );
           goal.order = 12;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           auto goal_handle = future_goal_handle.get(   );
          
           // Wait for the result
           auto future_result = node_->action_client_->async_get_result(  goal_handle );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // The final result
           rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
          
           // Sum all of the values in the requested fibonacci series
           int sum = 0;
           for (  auto number : result.result->sequence ) {
           sum += number;
           }
          
           EXPECT_EQ(  sum,   376 );
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_simple_action_with_feedback )
          {
           int feedback_sum = 0;
          
           // A callback to accumulate the intermediate values
           auto feedback_callback = [&feedback_sum](  
           rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr /*goal_handle*/,  
           const std::shared_ptr<const Fibonacci::Feedback> feedback )
           {
           feedback_sum += feedback->sequence.back(   );
           };
          
           // The goal for this invocation
           auto goal = Fibonacci::Goal(   );
           goal.order = 10;
          
           auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions(   );
           send_goal_options.feedback_callback = feedback_callback;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal,   send_goal_options );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           auto goal_handle = future_goal_handle.get(   );
          
           // Wait for the result
           auto future_result = node_->action_client_->async_get_result(  goal_handle );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_result ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // The final result
           rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
          
           // Sum all of the values in the requested fibonacci series
           int sum = 0;
           for (  auto number : result.result->sequence ) {
           sum += number;
           }
          
           EXPECT_EQ(  sum,   143 );
           EXPECT_GE(  feedback_sum,   0 ); // We should have received *some* feedback
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_simple_action_activation_cycling )
          {
           // The goal for this invocation
           auto goal = Fibonacci::Goal(   );
          
           // Sending a goal that will take a long time to calculate
           goal.order = 12'000'000;
          
           // Start by sending goal on an active server
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // Deactivate while running
           node_->deactivate_server(   );
          
           auto goal_handle = future_goal_handle.get(   );
          
           // Wait for the result
           auto future_result = node_->action_client_->async_get_result(  goal_handle );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // The action should be reported as aborted.
           EXPECT_EQ(  future_result.get(   ).code,   rclcpp_action::ResultCode::ABORTED );
          
           // Cycle back to active
           node_->activate_server(   );
          
           goal.order = 12;
          
           // Send the goal
           future_goal_handle = node_->action_client_->async_send_goal(  goal );
           std::cout << "Sent goal,   spinning til complete..." << std::endl;
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           goal_handle = future_goal_handle.get(   );
          
           // Wait for the result
           future_result = node_->action_client_->async_get_result(  goal_handle );
           std::cout << "Getting result,   spinning til complete..." << std::endl;
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // Now the action should have been successfully executed.
           EXPECT_EQ(  future_result.get(   ).code,   rclcpp_action::ResultCode::SUCCEEDED );
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_simple_action_preemption )
          {
           // The goal for this invocation
           auto goal = Fibonacci::Goal(   );
          
           // Sending a goal that will take a long time to calculate
           goal.order = 12'000'000;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           std::cout << "Sent goal,   spinning til complete..." << std::endl;
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // Preempt the goal
           auto preemption_goal = Fibonacci::Goal(   );
           preemption_goal.order = 1;
          
           // Send the goal
           future_goal_handle = node_->action_client_->async_send_goal(  preemption_goal );
           std::cout << "Sent goal,   spinning til complete..." << std::endl;
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           auto goal_handle = future_goal_handle.get(   );
          
           // Wait for the result
           auto future_result = node_->action_client_->async_get_result(  goal_handle );
           std::cout << "Getting result,   spinning til complete..." << std::endl;
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // The final result
           rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
          
           // Sum all of the values in the requested fibonacci series
           int sum = 0;
           for (  auto number : result.result->sequence ) {
           sum += number;
           }
          
           EXPECT_EQ(  sum,   1 );
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_simple_action_preemption_after_succeeded )
          {
           // Test race condition between successfully completing an action and receiving a preemption.
           auto goal = Fibonacci::Goal(   );
           goal.order = 20;
          
           auto preemption = Fibonacci::Goal(   );
           preemption.order = 1;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           node_->omit_server_preemptions(   );
          
           auto future_preempt_handle = node_->action_client_->async_send_goal(  preemption );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // Get the results
           auto goal_handle = future_goal_handle.get(   );
          
           // Wait for the result of initial goal
           auto future_result = node_->action_client_->async_get_result(  goal_handle );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // The final result
           rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
          
           // Sum all of the values in the requested fibonacci series
           int sum = 0;
           for (  auto number : result.result->sequence ) {
           sum += number;
           }
          
           EXPECT_EQ(  sum,   17710 );
          
           // Now get the preemption result
           goal_handle = future_preempt_handle.get(   );
          
           // Wait for the result of initial goal
           future_result = node_->action_client_->async_get_result(  goal_handle );
           ASSERT_EQ(  
           rclcpp::spin_until_future_complete(  node_,   future_result ),  
           rclcpp::FutureReturnCode::SUCCESS );
          
           // The final result
           result = future_result.get(   );
           EXPECT_EQ(  result.code,   rclcpp_action::ResultCode::SUCCEEDED );
          
           // Sum all of the values in the requested fibonacci series
           sum = 0;
           for (  auto number : result.result->sequence ) {
           sum += number;
           }
          
           EXPECT_EQ(  sum,   1 );
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_handle_goal_deactivated )
          {
           node_->deactivate_server(   );
           auto goal = Fibonacci::Goal(   );
           goal.order = 12;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           node_->activate_server(   );
          
           SUCCEED(   );
          }
          
          TEST_F(  ActionTest,   test_handle_cancel )
          {
           auto goal = Fibonacci::Goal(   );
           goal.order = 14000000;
          
           // Send the goal
           auto future_goal_handle = node_->action_client_->async_send_goal(  goal );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           future_goal_handle ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // Cancel the goal
           auto cancel_response = node_->action_client_->async_cancel_goal(  future_goal_handle.get(   ) );
           EXPECT_EQ(  
           rclcpp::spin_until_future_complete(  
           node_,  
           cancel_response ),   rclcpp::FutureReturnCode::SUCCESS );
          
           // Check cancelled
           EXPECT_EQ(  future_goal_handle.get(   )->get_status(   ),   rclcpp_action::GoalStatus::STATUS_CANCELING );
          
           SUCCEED(   );
          }
          
          int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           g_rclcppfixture.Setup(   );
           ::testing::InitGoogleTest(  &argc,   argv );
           auto result = RUN_ALL_TESTS(   );
           rclcpp::shutdown(   );
           rclcpp::Rate(  1 ).sleep(   );
           return result;
          }

navigation2/nav2_util/test/test_execution_timer.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <thread>
          
          #include "nav2_util/execution_timer.hpp"
          #include "gtest/gtest.h"
          
          using nav2_util::ExecutionTimer;
          using std::this_thread::sleep_for;
          using namespace std::chrono_literals;
          
      25  TEST(  ExecutionTimer,   BasicDelay )
          {
           ExecutionTimer t;
           t.start(   );
           sleep_for(  10ns );
           t.end(   );
           ASSERT_GE(  t.elapsed_time(   ),   10ns );
           ASSERT_GE(  t.elapsed_time_in_seconds(   ),   1e-8 );
          }

navigation2/nav2_util/test/test_geometry_utils.cpp

       1  // Copyright (  c ) 2018 Intel Corporation
          // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_util/geometry_utils.hpp"
          #include "geometry_msgs/msg/point.hpp"
          #include "geometry_msgs/msg/pose.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "gtest/gtest.h"
          
          using nav2_util::geometry_utils::euclidean_distance;
          using nav2_util::geometry_utils::calculate_path_length;
          
      25  TEST(  GeometryUtils,   euclidean_distance_point_3d )
          {
           geometry_msgs::msg::Point point1;
           point1.x = 3.0;
           point1.y = 2.0;
           point1.z = 1.0;
          
           geometry_msgs::msg::Point point2;
           point2.x = 1.0;
           point2.y = 2.0;
           point2.z = 3.0;
          
           ASSERT_NEAR(  euclidean_distance(  point1,   point2,   true ),   2.82843,   1e-5 );
          }
          
      40  TEST(  GeometryUtils,   euclidean_distance_point_2d )
          {
           geometry_msgs::msg::Point point1;
           point1.x = 3.0;
           point1.y = 2.0;
           point1.z = 1.0;
          
           geometry_msgs::msg::Point point2;
           point2.x = 1.0;
           point2.y = 2.0;
           point2.z = 3.0;
          
           ASSERT_NEAR(  euclidean_distance(  point1,   point2 ),   2.0,   1e-5 );
          }
          
      55  TEST(  GeometryUtils,   euclidean_distance_pose_3d )
          {
           geometry_msgs::msg::Pose pose1;
           pose1.position.x = 7.0;
           pose1.position.y = 4.0;
           pose1.position.z = 3.0;
          
           geometry_msgs::msg::Pose pose2;
           pose2.position.x = 17.0;
           pose2.position.y = 6.0;
           pose2.position.z = 2.0;
          
           ASSERT_NEAR(  euclidean_distance(  pose1,   pose2,   true ),   10.24695,   1e-5 );
          }
          
      70  TEST(  GeometryUtils,   euclidean_distance_pose_2d )
          {
           geometry_msgs::msg::Pose pose1;
           pose1.position.x = 7.0;
           pose1.position.y = 4.0;
           pose1.position.z = 3.0;
          
           geometry_msgs::msg::Pose pose2;
           pose2.position.x = 17.0;
           pose2.position.y = 6.0;
           pose2.position.z = 2.0;
          
           ASSERT_NEAR(  euclidean_distance(  pose1,   pose2 ),   10.19804,   1e-5 );
          }
          
      85  TEST(  GeometryUtils,   calculate_path_length )
          {
           nav_msgs::msg::Path straight_line_path;
           size_t nb_path_points = 10;
           float distance_between_poses = 2.0;
           float current_x_loc = 0.0;
          
           for (  size_t i = 0; i < nb_path_points; ++i ) {
           geometry_msgs::msg::PoseStamped pose_stamped_msg;
           pose_stamped_msg.pose.position.x = current_x_loc;
          
           straight_line_path.poses.push_back(  pose_stamped_msg );
          
           current_x_loc += distance_between_poses;
           }
          
           ASSERT_NEAR(  
           calculate_path_length(  straight_line_path ),  
           (  nb_path_points - 1 ) * distance_between_poses,   1e-5 );
          
           ASSERT_NEAR(  
           calculate_path_length(  straight_line_path,   straight_line_path.poses.size(   ) ),  
           0.0,   1e-5 );
          
           nav_msgs::msg::Path circle_path;
           float polar_distance = 2.0;
           uint32_t current_polar_angle_deg = 0;
           constexpr float pi = 3.14159265358979;
          
           while (  current_polar_angle_deg != 360 ) {
           float x_loc = polar_distance * std::cos(  current_polar_angle_deg * (  pi / 180.0 ) );
           float y_loc = polar_distance * std::sin(  current_polar_angle_deg * (  pi / 180.0 ) );
          
           geometry_msgs::msg::PoseStamped pose_stamped_msg;
           pose_stamped_msg.pose.position.x = x_loc;
           pose_stamped_msg.pose.position.y = y_loc;
          
           circle_path.poses.push_back(  pose_stamped_msg );
          
           current_polar_angle_deg += 1;
           }
          
           ASSERT_NEAR(  
           calculate_path_length(  circle_path ),  
           2 * pi * polar_distance,   1e-1 );
          }

navigation2/nav2_util/test/test_lifecycle_cli_node.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
          #define NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
          
          #include <cstdlib>
          #include <memory>
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/lifecycle_utils.hpp"
          #include "nav2_util/node_thread.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          #ifdef _WIN32
          #include <windows.h>
          #endif
          
      30  class DummyNode : public nav2_util::LifecycleNode
          {
          public:
      33   DummyNode(   )
           : nav2_util::LifecycleNode(  "nav2_test_cli",   "" )
           {
           activated = false;
           }
          
      39   nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & /*state*/ )
           {
           activated = true;
           return nav2_util::CallbackReturn::SUCCESS;
           }
          
      45   bool activated;
          };
          
      48  class Handle
          {
          public:
      51   Handle(   )
           {
           node = std::make_shared<DummyNode>(   );
           thread = std::make_shared<nav2_util::NodeThread>(  node->get_node_base_interface(   ) );
           }
      56   ~Handle(   )
           {
           thread.reset(   );
           node.reset(   );
           }
          
      62   std::shared_ptr<nav2_util::NodeThread> thread;
      63   std::shared_ptr<DummyNode> node;
          };
          
      66  class RclCppFixture
          {
          public:
      69   RclCppFixture(   )
           {
           rclcpp::init(  0,   nullptr );
           }
          
      74   ~RclCppFixture(   )
           {
           rclcpp::shutdown(   );
           }
          };
          
          RclCppFixture g_rclcppfixture;
          
      82  TEST(  LifeycleCLI,   fails_no_node_name )
          {
           Handle handle;
           auto rc = system(  "ros2 run nav2_util lifecycle_bringup" );
           (  void )rc;
          #ifdef _WIN32
           Sleep(  1000 );
          #else
           sleep(  1 );
          #endif
           // check node didn't mode
           EXPECT_EQ(  handle.node->activated,   false );
           SUCCEED(   );
          }
          
      97  TEST(  LifeycleCLI,   succeeds_node_name )
          {
           Handle handle;
           auto rc = system(  "ros2 run nav2_util lifecycle_bringup nav2_test_cli" );
          #ifdef _WIN32
           Sleep(  3000 );
          #else
           sleep(  3 );
          #endif
           // check node moved
           (  void )rc;
           EXPECT_EQ(  handle.node->activated,   true );
           SUCCEED(   );
          }
          
          #endif // NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_

navigation2/nav2_util/test/test_lifecycle_node.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      21  class RclCppFixture
          {
          public:
      24   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      25   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
          // For the following two tests,   if the LifecycleNode doesn't shut down properly,  
          // the overall test will hang since the rclcpp thread will still be running,  
          // preventing the executable from exiting (  the test will hang )
          
      33  TEST(  LifecycleNode,   RclcppNodeExitsCleanly )
          {
           // Make sure the node exits cleanly when using an rclcpp_node and associated thread
           auto node1 = std::make_shared<nav2_util::LifecycleNode>(  "test_node",   "" );
           std::this_thread::sleep_for(  std::chrono::seconds(  1 ) );
           SUCCEED(   );
          }
          
      41  TEST(  LifecycleNode,   MultipleRclcppNodesExitCleanly )
          {
           // Try a couple nodes w/ rclcpp_node and threads
           auto node1 = std::make_shared<nav2_util::LifecycleNode>(  "test_node1",   "" );
           auto node2 = std::make_shared<nav2_util::LifecycleNode>(  "test_node2",   "" );
          
           std::this_thread::sleep_for(  std::chrono::seconds(  1 ) );
           SUCCEED(   );
          }

navigation2/nav2_util/test/test_lifecycle_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <thread>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_utils.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          using nav2_util::startup_lifecycle_nodes;
          using nav2_util::reset_lifecycle_nodes;
          
      26  class RclCppFixture
          {
          public:
      29   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      30   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      34  void SpinNodesUntilDone(  
           std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes,  
           std::atomic<bool> * test_done )
          {
           rclcpp::executors::SingleThreadedExecutor exec;
           for (  const auto & node : nodes ) {
           exec.add_node(  node->get_node_base_interface(   ) );
           }
           while (  rclcpp::ok(   ) && !(  *test_done ) ) {
           exec.spin_some(   );
           }
          }
          
      47  TEST(  Lifecycle,   interface )
          {
           std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes;
           nodes.push_back(  rclcpp_lifecycle::LifecycleNode::make_shared(  "foo" ) );
           nodes.push_back(  rclcpp_lifecycle::LifecycleNode::make_shared(  "bar" ) );
          
           std::atomic<bool> done(  false );
           std::thread node_thread(  SpinNodesUntilDone,   nodes,   &done );
           startup_lifecycle_nodes(  "/foo:/bar" );
           reset_lifecycle_nodes(  "/foo:/bar" );
           done = true;
           node_thread.join(   );
           SUCCEED(   );
          }

navigation2/nav2_util/test/test_node_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          
          #include "nav2_util/node_utils.hpp"
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          
          using nav2_util::sanitize_node_name;
          using nav2_util::generate_internal_node_name;
          using nav2_util::generate_internal_node;
          using nav2_util::add_namespaces;
          using nav2_util::time_to_string;
          using nav2_util::declare_parameter_if_not_declared;
          using nav2_util::get_plugin_type_param;
          
      30  class RclCppFixture
          {
          public:
      33   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      34   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      38  TEST(  SanitizeNodeName,   SanitizeNodeName )
          {
           ASSERT_EQ(  sanitize_node_name(  "bar" ),   "bar" );
           ASSERT_EQ(  sanitize_node_name(  "/foo/bar" ),   "_foo_bar" );
          }
          
      44  TEST(  TimeToString,   IsLengthCorrect )
          {
           ASSERT_EQ(  time_to_string(  0 ).length(   ),   0u );
           ASSERT_EQ(  time_to_string(  1 ).length(   ),   1u );
           ASSERT_EQ(  time_to_string(  10 ).length(   ),   10u );
           ASSERT_EQ(  time_to_string(  20 )[0],   '0' );
          }
          
      52  TEST(  TimeToString,   TimeToStringDifferent )
          {
           auto time1 = time_to_string(  8 );
           auto time2 = time_to_string(  8 );
           ASSERT_NE(  time1,   time2 );
          }
          
      59  TEST(  GenerateInternalNodeName,   GenerateNodeName )
          {
           auto defaultName = generate_internal_node_name(   );
           ASSERT_EQ(  defaultName[0],   '_' );
           ASSERT_EQ(  defaultName.length(   ),   9u );
          }
          
      66  TEST(  AddNamespaces,   AddNamespaceSlash )
          {
           ASSERT_EQ(  add_namespaces(  "hi",   "bye" ),   "hi/bye" );
           ASSERT_EQ(  add_namespaces(  "hi/",   "bye" ),   "/hi/bye" );
          }
          
      72  TEST(  DeclareParameterIfNotDeclared,   DeclareParameterIfNotDeclared )
          {
           auto node = std::make_shared<rclcpp::Node>(  "test_node" );
           std::string param;
          
           // test declared parameter
           node->declare_parameter(  "foobar",   "foo" );
           declare_parameter_if_not_declared(  node,   "foobar",   rclcpp::ParameterValue{"bar"} );
           node->get_parameter(  "foobar",   param );
           ASSERT_EQ(  param,   "foo" );
          
           // test undeclared parameter
           declare_parameter_if_not_declared(  node,   "waldo",   rclcpp::ParameterValue{"fred"} );
           node->get_parameter(  "waldo",   param );
           ASSERT_EQ(  param,   "fred" );
          }
          
      89  TEST(  GetPluginTypeParam,   GetPluginTypeParam )
          {
           ::testing::FLAGS_gtest_death_test_style = "threadsafe";
           auto node = std::make_shared<rclcpp::Node>(  "test_node" );
           node->declare_parameter(  "Foo.plugin",   "bar" );
           ASSERT_EQ(  get_plugin_type_param(  node,   "Foo" ),   "bar" );
           ASSERT_EXIT(  get_plugin_type_param(  node,   "Waldo" ),   ::testing::ExitedWithCode(  255 ),   ".*" );
          }

navigation2/nav2_util/test/test_odometry_utils.cpp

       1  // Copyright (  c ) 2020 Sarthak Mittal
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <chrono>
          
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/odometry_utils.hpp"
          #include "nav_msgs/msg/odometry.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          #include "gtest/gtest.h"
          
          using namespace std::chrono; // NOLINT
          using namespace std::chrono_literals; // NOLINT
          
      27  class RclCppFixture
          {
          public:
      30   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      31   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      35  TEST(  OdometryUtils,   test_smoothed_velocity )
          {
           auto node = std::make_shared<rclcpp::Node>(  "test_node" );
           auto odom_pub = node->create_publisher<nav_msgs::msg::Odometry>(  "odom",   1 );
          
           nav2_util::OdomSmoother odom_smoother(  node,   0.3,   "odom" );
          
           nav_msgs::msg::Odometry odom_msg;
           geometry_msgs::msg::Twist twist_msg;
          
           auto time = node->now(   );
          
           odom_msg.header.stamp = time;
           odom_msg.twist.twist.linear.x = 1.0;
           odom_msg.twist.twist.linear.y = 1.0;
           odom_msg.twist.twist.angular.z = 1.0;
          
           odom_pub->publish(  odom_msg );
           rclcpp::spin_some(  node );
          
           twist_msg = odom_smoother.getTwist(   );
           EXPECT_EQ(  twist_msg.linear.x,   1.0 );
           EXPECT_EQ(  twist_msg.linear.y,   1.0 );
           EXPECT_EQ(  twist_msg.angular.z,   1.0 );
          
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.1 );
           odom_msg.twist.twist.linear.x = 2.0;
           odom_msg.twist.twist.linear.y = 2.0;
           odom_msg.twist.twist.angular.z = 2.0;
           odom_pub->publish(  odom_msg );
          
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node );
          
           twist_msg = odom_smoother.getTwist(   );
           EXPECT_EQ(  twist_msg.linear.x,   1.5 );
           EXPECT_EQ(  twist_msg.linear.y,   1.5 );
           EXPECT_EQ(  twist_msg.angular.z,   1.5 );
          
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.2 );
           odom_msg.twist.twist.linear.x = 3.0;
           odom_msg.twist.twist.linear.y = 3.0;
           odom_msg.twist.twist.angular.z = 3.0;
           odom_pub->publish(  odom_msg );
          
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node );
          
           twist_msg = odom_smoother.getTwist(   );
           EXPECT_EQ(  twist_msg.linear.x,   2.0 );
           EXPECT_EQ(  twist_msg.linear.y,   2.0 );
           EXPECT_EQ(  twist_msg.angular.z,   2.0 );
          
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  0.45 );
           odom_msg.twist.twist.linear.x = 4.0;
           odom_msg.twist.twist.linear.y = 4.0;
           odom_msg.twist.twist.angular.z = 4.0;
           odom_pub->publish(  odom_msg );
          
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node );
          
           twist_msg = odom_smoother.getTwist(   );
           EXPECT_EQ(  twist_msg.linear.x,   3.5 );
           EXPECT_EQ(  twist_msg.linear.y,   3.5 );
           EXPECT_EQ(  twist_msg.angular.z,   3.5 );
          
           odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(  1.0 );
           odom_msg.twist.twist.linear.x = 5.0;
           odom_msg.twist.twist.linear.y = 5.0;
           odom_msg.twist.twist.angular.z = 5.0;
           odom_pub->publish(  odom_msg );
          
           std::this_thread::sleep_for(  100ms );
           rclcpp::spin_some(  node );
          
           twist_msg = odom_smoother.getTwist(   );
           EXPECT_EQ(  twist_msg.linear.x,   5.0 );
           EXPECT_EQ(  twist_msg.linear.y,   5.0 );
           EXPECT_EQ(  twist_msg.angular.z,   5.0 );
          }

navigation2/nav2_util/test/test_robot_utils.cpp

       1  // Copyright (  c ) 2020 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/robot_utils.hpp"
          #include "tf2_ros/transform_listener.h"
          #include "tf2_ros/transform_broadcaster.h"
          #include "geometry_msgs/msg/pose_stamped.hpp"
          #include "gtest/gtest.h"
          #include "nav2_util/node_thread.hpp"
          #include "tf2_ros/create_timer_ros.h"
          
      25  TEST(  RobotUtils,   LookupExceptionError )
          {
           rclcpp::init(  0,   nullptr );
           auto node = std::make_shared<rclcpp::Node>(  "name",   rclcpp::NodeOptions(   ) );
           geometry_msgs::msg::PoseStamped global_pose;
           tf2_ros::Buffer tf(  node->get_clock(   ) );
           ASSERT_FALSE(  nav2_util::getCurrentPose(  global_pose,   tf,   "map",   "base_link",   0.1 ) );
           global_pose.header.frame_id = "base_link";
           ASSERT_FALSE(  nav2_util::transformPoseInTargetFrame(  global_pose,   global_pose,   tf,   "map",   0.1 ) );
          }

navigation2/nav2_util/test/test_service_client.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <string>
          #include "nav2_util/service_client.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "std_srvs/srv/empty.hpp"
          #include "std_msgs/msg/empty.hpp"
          #include "gtest/gtest.h"
          
          using nav2_util::ServiceClient;
          using std::string;
          
      26  class RclCppFixture
          {
          public:
      29   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      30   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      34  class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
          {
          public:
      37   TestServiceClient(  
           const std::string & name,  
           const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr(   ) )
           : ServiceClient(  name,   provided_node ) {}
          
      42   string name(   ) {return node_->get_name(   );}
      43   const rclcpp::Node::SharedPtr & getNode(   ) {return node_;}
          };
          
      46  TEST(  ServiceClient,   can_ServiceClient_use_passed_in_node )
          {
           auto node = rclcpp::Node::make_shared(  "test_node" );
           TestServiceClient t(  "bar",   node );
           ASSERT_EQ(  t.getNode(   ),   node );
           ASSERT_EQ(  t.name(   ),   "test_node" );
          }
          
      54  TEST(  ServiceClient,   can_ServiceClient_invoke_in_callback )
          {
           int a = 0;
           auto service_node = rclcpp::Node::make_shared(  "service_node" );
           auto service = service_node->create_service<std_srvs::srv::Empty>(  
           "empty_srv",  
           [&a](  std_srvs::srv::Empty::Request::SharedPtr,   std_srvs::srv::Empty::Response::SharedPtr ) {
           a = 1;
           } );
           auto srv_thread = std::thread(  [&](   ) {rclcpp::spin(  service_node );} );
          
           auto pub_node = rclcpp::Node::make_shared(  "pub_node" );
           auto pub = pub_node->create_publisher<std_msgs::msg::Empty>(  
           "empty_topic",  
           rclcpp::QoS(  1 ).transient_local(   ) );
           auto pub_thread = std::thread(  [&](   ) {rclcpp::spin(  pub_node );} );
          
           auto sub_node = rclcpp::Node::make_shared(  "sub_node" );
           ServiceClient<std_srvs::srv::Empty> client(  "empty_srv",   sub_node );
           auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(  
           "empty_topic",  
           rclcpp::QoS(  1 ),  
           [&client](  std_msgs::msg::Empty::SharedPtr ) {
           auto req = std::make_shared<std_srvs::srv::Empty::Request>(   );
           auto res = client.invoke(  req );
           } );
          
           pub->publish(  std_msgs::msg::Empty(   ) );
           rclcpp::spin_some(  sub_node );
          
           rclcpp::shutdown(   );
           srv_thread.join(   );
           pub_thread.join(   );
           ASSERT_EQ(  a,   1 );
          }
          
      90  TEST(  ServiceClient,   can_ServiceClient_timeout )
          {
           rclcpp::init(  0,   nullptr );
           auto node = rclcpp::Node::make_shared(  "test_node" );
           TestServiceClient t(  "bar",   node );
           rclcpp::spin_some(  node );
           bool ready = t.wait_for_service(  std::chrono::milliseconds(  10 ) );
           rclcpp::shutdown(   );
           ASSERT_EQ(  ready,   false );
          }

navigation2/nav2_util/test/test_string_utils.cpp

       1  // Copyright (  c ) 2019 Intel Corporation
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "nav2_util/string_utils.hpp"
          #include "gtest/gtest.h"
          
          using nav2_util::split;
          using nav2_util::Tokens;
          
      23  TEST(  Split,   SplitFunction )
          {
           ASSERT_EQ(  split(  "",   ':' ),   Tokens(  {""} ) );
           ASSERT_EQ(  split(  "foo",   ':' ),   Tokens{"foo"} );
           ASSERT_EQ(  split(  "foo:bar",   ':' ),   Tokens(  {"foo",   "bar"} ) );
           ASSERT_EQ(  split(  "foo:bar:",   ':' ),   Tokens(  {"foo",   "bar",   ""} ) );
           ASSERT_EQ(  split(  ":",   ':' ),   Tokens(  {"",   ""} ) );
           ASSERT_EQ(  split(  "foo::bar",   ':' ),   Tokens(  {"foo",   "",   "bar"} ) );
           ASSERT_TRUE(  nav2_util::strip_leading_slash(  std::string(  "/hi" ) ) == std::string(  "hi" ) );
          }

navigation2/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp

          // Copyright (  c ) 2022 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
          #define NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
          
          #include <chrono>
          #include <limits>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_util/node_utils.hpp"
          #include "nav2_util/odometry_utils.hpp"
          
          namespace nav2_velocity_smoother
          {
          
          /**
           * @class nav2_velocity_smoother::VelocitySmoother
           * @brief This class that smooths cmd_vel velocities for robot bases
           */
      36  class VelocitySmoother : public nav2_util::LifecycleNode
          {
          public:
           /**
           * @brief A constructor for nav2_velocity_smoother::VelocitySmoother
           * @param options Additional options to control creation of the node.
           */
      43   explicit VelocitySmoother(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
          
           /**
           * @brief Destructor for nav2_velocity_smoother::VelocitySmoother
           */
      48   ~VelocitySmoother(   );
          
           /**
           * @brief Find the scale factor,   eta,   which scales axis into acceleration range
           * @param v_curr current velocity
           * @param v_cmd commanded velocity
           * @param accel maximum acceleration
           * @param decel maximum deceleration
           * @return Scale factor,   eta
           */
      58   double findEtaConstraint(  
           const double v_curr,   const double v_cmd,  
           const double accel,   const double decel );
          
           /**
           * @brief Apply acceleration and scale factor constraints
           * @param v_curr current velocity
           * @param v_cmd commanded velocity
           * @param accel maximum acceleration
           * @param decel maximum deceleration
           * @param eta Scale factor
           * @return Velocity command
           */
      71   double applyConstraints(  
           const double v_curr,   const double v_cmd,  
           const double accel,   const double decel,   const double eta );
          
          protected:
           /**
           * @brief Configures parameters and member variables
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Activates member variables
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Deactivates member variables
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Calls clean up states and resets member variables.
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Called when in Shutdown state
           * @param state LifeCycle Node's state
           * @return Success or Failure
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Callback for incoming velocity commands
           * @param msg Twist message
           */
           void inputCommandCallback(  const geometry_msgs::msg::Twist::SharedPtr msg );
          
           /**
           * @brief Main worker timer function
           */
           void smootherTimer(   );
          
           /**
           * @brief Dynamic reconfigure callback
           * @param parameters Parameter list to change
           */
           rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(  
           std::vector<rclcpp::Parameter> parameters );
          
           // Network interfaces
           std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
           rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr
           smoothed_cmd_pub_;
           rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
           rclcpp::TimerBase::SharedPtr timer_;
          
           rclcpp::Clock::SharedPtr clock_;
           geometry_msgs::msg::Twist last_cmd_;
           geometry_msgs::msg::Twist::SharedPtr command_;
          
           // Parameters
           double smoothing_frequency_;
           double odom_duration_;
           std::string odom_topic_;
           bool open_loop_;
           bool stopped_{true};
           bool scale_velocities_;
           std::vector<double> max_velocities_;
           std::vector<double> min_velocities_;
           std::vector<double> max_accels_;
           std::vector<double> max_decels_;
           std::vector<double> deadband_velocities_;
           rclcpp::Duration velocity_timeout_{0,   0};
           rclcpp::Time last_command_time_;
          
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          };
          
          } // namespace nav2_velocity_smoother
          
          #endif // NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_

navigation2/nav2_velocity_smoother/src/main.cpp

       1  // Copyright (  c ) 2022 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_velocity_smoother/velocity_smoother.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_velocity_smoother::VelocitySmoother>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_velocity_smoother/src/velocity_smoother.cpp

          // Copyright (  c ) 2022 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <limits>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "nav2_velocity_smoother/velocity_smoother.hpp"
          
          using namespace std::chrono_literals;
          using nav2_util::declare_parameter_if_not_declared;
          using std::placeholders::_1;
          using rcl_interfaces::msg::ParameterType;
          
          namespace nav2_velocity_smoother
          {
          
      32  VelocitySmoother::VelocitySmoother(  const rclcpp::NodeOptions & options )
          : LifecycleNode(  "velocity_smoother",   "",   options ),  
           last_command_time_{0,   0,   get_clock(   )->get_clock_type(   )}
          {
          }
          
          VelocitySmoother::~VelocitySmoother(   )
          {
           if (  timer_ ) {
           timer_->cancel(   );
           timer_.reset(   );
           }
          }
          
          nav2_util::CallbackReturn
          VelocitySmoother::on_configure(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring velocity smoother" );
           auto node = shared_from_this(   );
           std::string feedback_type;
           double velocity_timeout_dbl;
          
           // Smoothing metadata
           declare_parameter_if_not_declared(  node,   "smoothing_frequency",   rclcpp::ParameterValue(  20.0 ) );
           declare_parameter_if_not_declared(  
           node,   "feedback",   rclcpp::ParameterValue(  std::string(  "OPEN_LOOP" ) ) );
           declare_parameter_if_not_declared(  node,   "scale_velocities",   rclcpp::ParameterValue(  false ) );
           node->get_parameter(  "smoothing_frequency",   smoothing_frequency_ );
           node->get_parameter(  "feedback",   feedback_type );
           node->get_parameter(  "scale_velocities",   scale_velocities_ );
          
           // Kinematics
           declare_parameter_if_not_declared(  
           node,   "max_velocity",   rclcpp::ParameterValue(  std::vector<double>{0.50,   0.0,   2.5} ) );
           declare_parameter_if_not_declared(  
           node,   "min_velocity",   rclcpp::ParameterValue(  std::vector<double>{-0.50,   0.0,   -2.5} ) );
           declare_parameter_if_not_declared(  
           node,   "max_accel",   rclcpp::ParameterValue(  std::vector<double>{2.5,   0.0,   3.2} ) );
           declare_parameter_if_not_declared(  
           node,   "max_decel",   rclcpp::ParameterValue(  std::vector<double>{-2.5,   0.0,   -3.2} ) );
           node->get_parameter(  "max_velocity",   max_velocities_ );
           node->get_parameter(  "min_velocity",   min_velocities_ );
           node->get_parameter(  "max_accel",   max_accels_ );
           node->get_parameter(  "max_decel",   max_decels_ );
          
           for (  unsigned int i = 0; i != max_decels_.size(   ); i++ ) {
           if (  max_decels_[i] > 0.0 ) {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Positive values set of deceleration! These should be negative to slow down!" );
           }
           }
          
           // Get feature parameters
           declare_parameter_if_not_declared(  node,   "odom_topic",   rclcpp::ParameterValue(  "odom" ) );
           declare_parameter_if_not_declared(  node,   "odom_duration",   rclcpp::ParameterValue(  0.1 ) );
           declare_parameter_if_not_declared(  
           node,   "deadband_velocity",   rclcpp::ParameterValue(  std::vector<double>{0.0,   0.0,   0.0} ) );
           declare_parameter_if_not_declared(  node,   "velocity_timeout",   rclcpp::ParameterValue(  1.0 ) );
           node->get_parameter(  "odom_topic",   odom_topic_ );
           node->get_parameter(  "odom_duration",   odom_duration_ );
           node->get_parameter(  "deadband_velocity",   deadband_velocities_ );
           node->get_parameter(  "velocity_timeout",   velocity_timeout_dbl );
           velocity_timeout_ = rclcpp::Duration::from_seconds(  velocity_timeout_dbl );
          
           if (  max_velocities_.size(   ) != 3 || min_velocities_.size(   ) != 3 ||
           max_accels_.size(   ) != 3 || max_decels_.size(   ) != 3 || deadband_velocities_.size(   ) != 3 )
           {
           throw std::runtime_error(  
           "Invalid setting of kinematic and/or deadband limits!"
           " All limits must be size of 3 representing (  x,   y,   theta )." );
           }
          
           // Get control type
           if (  feedback_type == "OPEN_LOOP" ) {
           open_loop_ = true;
           } else if (  feedback_type == "CLOSED_LOOP" ) {
           open_loop_ = false;
           odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(  node,   odom_duration_,   odom_topic_ );
           } else {
           throw std::runtime_error(  "Invalid feedback_type,   options are OPEN_LOOP and CLOSED_LOOP." );
           }
          
           // Setup inputs / outputs
           smoothed_cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>(  "cmd_vel_smoothed",   1 );
           cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(  
           "cmd_vel",   rclcpp::QoS(  1 ),  
           std::bind(  &VelocitySmoother::inputCommandCallback,   this,   std::placeholders::_1 ) );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          VelocitySmoother::on_activate(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
           smoothed_cmd_pub_->on_activate(   );
           double timer_duration_ms = 1000.0 / smoothing_frequency_;
           timer_ = create_wall_timer(  
           std::chrono::milliseconds(  static_cast<int>(  timer_duration_ms ) ),  
           std::bind(  &VelocitySmoother::smootherTimer,   this ) );
          
           dyn_params_handler_ = this->add_on_set_parameters_callback(  
           std::bind(  &VelocitySmoother::dynamicParametersCallback,   this,   _1 ) );
          
           // create bond connection
           createBond(   );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          VelocitySmoother::on_deactivate(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
           if (  timer_ ) {
           timer_->cancel(   );
           timer_.reset(   );
           }
           smoothed_cmd_pub_->on_deactivate(   );
           dyn_params_handler_.reset(   );
          
           // destroy bond connection
           destroyBond(   );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          VelocitySmoother::on_cleanup(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
           smoothed_cmd_pub_.reset(   );
           odom_smoother_.reset(   );
           cmd_sub_.reset(   );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
          VelocitySmoother::on_shutdown(  const rclcpp_lifecycle::State & )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          void VelocitySmoother::inputCommandCallback(  const geometry_msgs::msg::Twist::SharedPtr msg )
          {
           command_ = msg;
           last_command_time_ = now(   );
          }
          
          double VelocitySmoother::findEtaConstraint(  
           const double v_curr,   const double v_cmd,   const double accel,   const double decel )
          {
           // Exploiting vector scaling properties
           const double v_component_max = accel / smoothing_frequency_;
           const double v_component_min = decel / smoothing_frequency_;
           const double dv = v_cmd - v_curr;
          
           if (  dv > v_component_max ) {
           return v_component_max / dv;
           }
          
           if (  dv < v_component_min ) {
           return v_component_min / dv;
           }
          
           return -1.0;
          }
          
          double VelocitySmoother::applyConstraints(  
           const double v_curr,   const double v_cmd,  
           const double accel,   const double decel,   const double eta )
          {
           double dv = v_cmd - v_curr;
           const double v_component_max = accel / smoothing_frequency_;
           const double v_component_min = decel / smoothing_frequency_;
           return v_curr + std::clamp(  eta * dv,   v_component_min,   v_component_max );
          }
          
          void VelocitySmoother::smootherTimer(   )
          {
           auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(   );
          
           // Check for velocity timeout. If nothing received,   publish zeros to stop robot
           if (  now(   ) - last_command_time_ > velocity_timeout_ ) {
           last_cmd_ = geometry_msgs::msg::Twist(   );
           if (  !stopped_ ) {
           smoothed_cmd_pub_->publish(  std::move(  cmd_vel ) );
           }
           stopped_ = true;
           return;
           }
          
           stopped_ = false;
          
           // Get current velocity based on feedback type
           geometry_msgs::msg::Twist current_;
           if (  open_loop_ ) {
           current_ = last_cmd_;
           } else {
           current_ = odom_smoother_->getTwist(   );
           }
          
           // Apply absolute velocity restrictions to the command
           command_->linear.x = std::clamp(  command_->linear.x,   min_velocities_[0],   max_velocities_[0] );
           command_->linear.y = std::clamp(  command_->linear.y,   min_velocities_[1],   max_velocities_[1] );
           command_->angular.z = std::clamp(  command_->angular.z,   min_velocities_[2],   max_velocities_[2] );
          
           // Find if any component is not within the acceleration constraints. If so,   store the most
           // significant scale factor to apply to the vector <dvx,   dvy,   dvw>,   eta,   to reduce all axes
           // proportionally to follow the same direction,   within change of velocity bounds.
           // In case eta reduces another axis out of its own limit,   apply accel constraint to guarantee
           // output is within limits,   even if it deviates from requested command slightly.
           double eta = 1.0;
           if (  scale_velocities_ ) {
           double curr_eta = -1.0;
          
           curr_eta = findEtaConstraint(  
           current_.linear.x,   command_->linear.x,   max_accels_[0],   max_decels_[0] );
           if (  curr_eta > 0.0 && std::fabs(  1.0 - curr_eta ) > std::fabs(  1.0 - eta ) ) {
           eta = curr_eta;
           }
          
           curr_eta = findEtaConstraint(  
           current_.linear.y,   command_->linear.y,   max_accels_[1],   max_decels_[1] );
           if (  curr_eta > 0.0 && std::fabs(  1.0 - curr_eta ) > std::fabs(  1.0 - eta ) ) {
           eta = curr_eta;
           }
          
           curr_eta = findEtaConstraint(  
           current_.angular.z,   command_->angular.z,   max_accels_[2],   max_decels_[2] );
           if (  curr_eta > 0.0 && std::fabs(  1.0 - curr_eta ) > std::fabs(  1.0 - eta ) ) {
           eta = curr_eta;
           }
           }
          
           cmd_vel->linear.x = applyConstraints(  
           current_.linear.x,   command_->linear.x,   max_accels_[0],   max_decels_[0],   eta );
           cmd_vel->linear.y = applyConstraints(  
           current_.linear.y,   command_->linear.y,   max_accels_[1],   max_decels_[1],   eta );
           cmd_vel->angular.z = applyConstraints(  
           current_.angular.z,   command_->angular.z,   max_accels_[2],   max_decels_[2],   eta );
          
           // If open loop,   assume we achieved it
           if (  open_loop_ ) {
           last_cmd_ = *cmd_vel;
           }
          
           // Apply deadband restrictions & publish
           cmd_vel->linear.x = fabs(  cmd_vel->linear.x ) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x;
           cmd_vel->linear.y = fabs(  cmd_vel->linear.y ) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y;
           cmd_vel->angular.z = fabs(  cmd_vel->angular.z ) <
           deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z;
           smoothed_cmd_pub_->publish(  std::move(  cmd_vel ) );
          }
          
          rcl_interfaces::msg::SetParametersResult
          VelocitySmoother::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           rcl_interfaces::msg::SetParametersResult result;
           result.successful = true;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_DOUBLE ) {
           if (  name == "smoothing_frequency" ) {
           smoothing_frequency_ = parameter.as_double(   );
           if (  timer_ ) {
           timer_->cancel(   );
           timer_.reset(   );
           }
          
           double timer_duration_ms = 1000.0 / smoothing_frequency_;
           timer_ = create_wall_timer(  
           std::chrono::milliseconds(  static_cast<int>(  timer_duration_ms ) ),  
           std::bind(  &VelocitySmoother::smootherTimer,   this ) );
           } else if (  name == "velocity_timeout" ) {
           velocity_timeout_ = rclcpp::Duration::from_seconds(  parameter.as_double(   ) );
           } else if (  name == "odom_duration" ) {
           odom_duration_ = parameter.as_double(   );
           odom_smoother_ =
           std::make_unique<nav2_util::OdomSmoother>(  
           shared_from_this(   ),   odom_duration_,   odom_topic_ );
           }
           } else if (  type == ParameterType::PARAMETER_DOUBLE_ARRAY ) {
           if (  parameter.as_double_array(   ).size(   ) != 3 ) {
           RCLCPP_WARN(  get_logger(   ),   "Invalid size of parameter %s. Must be size 3",   name.c_str(   ) );
           result.successful = false;
           break;
           }
          
           if (  name == "max_velocity" ) {
           max_velocities_ = parameter.as_double_array(   );
           } else if (  name == "min_velocity" ) {
           min_velocities_ = parameter.as_double_array(   );
           } else if (  name == "max_accel" ) {
           max_accels_ = parameter.as_double_array(   );
           } else if (  name == "max_decel" ) {
           max_decels_ = parameter.as_double_array(   );
           } else if (  name == "deadband_velocity" ) {
           deadband_velocities_ = parameter.as_double_array(   );
           }
           } else if (  type == ParameterType::PARAMETER_STRING ) {
           if (  name == "feedback" ) {
           if (  parameter.as_string(   ) == "OPEN_LOOP" ) {
           open_loop_ = true;
           odom_smoother_.reset(   );
           } else if (  parameter.as_string(   ) == "CLOSED_LOOP" ) {
           open_loop_ = false;
           odom_smoother_ =
           std::make_unique<nav2_util::OdomSmoother>(  
           shared_from_this(   ),   odom_duration_,   odom_topic_ );
           } else {
           RCLCPP_WARN(  
           get_logger(   ),   "Invalid feedback_type,   options are OPEN_LOOP and CLOSED_LOOP." );
           result.successful = false;
           break;
           }
           } else if (  name == "odom_topic" ) {
           odom_topic_ = parameter.as_string(   );
           odom_smoother_ =
           std::make_unique<nav2_util::OdomSmoother>(  
           shared_from_this(   ),   odom_duration_,   odom_topic_ );
           }
           }
           }
          
           return result;
          }
          
          } // namespace nav2_velocity_smoother
          
          #include "rclcpp_components/register_node_macro.hpp"
          RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_velocity_smoother::VelocitySmoother )

navigation2/nav2_velocity_smoother/test/test_velocity_smoother.cpp

       1  // Copyright (  c ) 2022 Samsung Research
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <math.h>
          #include <memory>
          #include <string>
          #include <vector>
          #include <limits>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_velocity_smoother/velocity_smoother.hpp"
          #include "nav_msgs/msg/odometry.hpp"
          #include "geometry_msgs/msg/twist.hpp"
          
          using namespace std::chrono_literals;
          
      29  class RclCppFixture
          {
          public:
      32   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      33   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      37  class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
          {
          public:
      40   VelSmootherShim(   )
           : VelocitySmoother(   ) {}
      42   void configure(  const rclcpp_lifecycle::State & state ) {this->on_configure(  state );}
      43   void activate(  const rclcpp_lifecycle::State & state ) {this->on_activate(  state );}
      44   void deactivate(  const rclcpp_lifecycle::State & state ) {this->on_deactivate(  state );}
      45   void cleanup(  const rclcpp_lifecycle::State & state ) {this->on_cleanup(  state );}
      46   void shutdown(  const rclcpp_lifecycle::State & state ) {this->on_shutdown(  state );}
          
      48   bool isOdomSmoother(   ) {return odom_smoother_ ? true : false;}
      49   bool hasCommandMsg(   ) {return last_command_time_.nanoseconds(   ) != 0;}
      50   geometry_msgs::msg::Twist::SharedPtr lastCommandMsg(   ) {return command_;}
          
      52   void sendCommandMsg(  geometry_msgs::msg::Twist::SharedPtr msg ) {inputCommandCallback(  msg );}
          };
          
      55  TEST(  VelocitySmootherTest,   openLoopTestTimer )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           std::vector<double> deadbands{0.2,   0.0,   0.0};
           smoother->declare_parameter(  "scale_velocities",   rclcpp::ParameterValue(  true ) );
           smoother->set_parameter(  rclcpp::Parameter(  "scale_velocities",   true ) );
           smoother->declare_parameter(  "deadband_velocity",   rclcpp::ParameterValue(  deadbands ) );
           smoother->set_parameter(  rclcpp::Parameter(  "deadband_velocity",   deadbands ) );
           rclcpp_lifecycle::State state;
           smoother->configure(  state );
           smoother->activate(  state );
          
           std::vector<double> linear_vels;
           auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(  
           "cmd_vel_smoothed",  
           1,  
           [&](  geometry_msgs::msg::Twist::SharedPtr msg ) {
           linear_vels.push_back(  msg->linear.x );
           } );
          
           // Send a velocity command
           auto cmd = std::make_shared<geometry_msgs::msg::Twist>(   );
           cmd->linear.x = 1.0; // Max is 0.5,   so should threshold
           smoother->sendCommandMsg(  cmd );
          
           // Process velocity smoothing and send updated odometry based on commands
           auto start = smoother->now(   );
           while (  smoother->now(   ) - start < 1.5s ) {
           rclcpp::spin_some(  smoother->get_node_base_interface(   ) );
           }
          
           // Sanity check we have the approximately right number of messages for the timespan and timeout
           EXPECT_GT(  linear_vels.size(   ),   19u );
           EXPECT_LT(  linear_vels.size(   ),   30u );
          
           // Should have last command be a stop since we timed out the command stream
           EXPECT_EQ(  linear_vels.back(   ),   0.0 );
          
           // From deadband,   first few should be 0 until above 0.2
           for (  unsigned int i = 0; i != linear_vels.size(   ); i++ ) {
           if (  linear_vels[i] != 0 ) {
           EXPECT_GT(  linear_vels[i],   0.2 );
           break;
           }
           }
          
           // Process to make sure stops at limit in velocity,  
           // doesn't exceed acceleration
           for (  unsigned int i = 0; i != linear_vels.size(   ); i++ ) {
           EXPECT_TRUE(  linear_vels[i] <= 0.5 );
           }
          }
          
     109  TEST(  VelocitySmootherTest,   approxClosedLoopTestTimer )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           smoother->declare_parameter(  "feedback",   rclcpp::ParameterValue(  std::string(  "CLOSED_LOOP" ) ) );
           smoother->set_parameter(  rclcpp::Parameter(  "feedback",   std::string(  "CLOSED_LOOP" ) ) );
           rclcpp_lifecycle::State state;
           smoother->configure(  state );
           smoother->activate(  state );
          
           std::vector<double> linear_vels;
           auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(  
           "cmd_vel_smoothed",  
           1,  
           [&](  geometry_msgs::msg::Twist::SharedPtr msg ) {
           linear_vels.push_back(  msg->linear.x );
           } );
          
           auto odom_pub = smoother->create_publisher<nav_msgs::msg::Odometry>(  "odom",   1 );
           odom_pub->on_activate(   );
           nav_msgs::msg::Odometry odom_msg;
           odom_msg.header.frame_id = "odom";
           odom_msg.child_frame_id = "base_link";
          
           // Fill buffer with 0 twisted-commands
           for (  unsigned int i = 0; i != 30; i++ ) {
           odom_msg.header.stamp = smoother->now(   ) + rclcpp::Duration::from_seconds(  i * 0.01 );
           odom_pub->publish(  odom_msg );
           }
          
           // Send a velocity command
           auto cmd = std::make_shared<geometry_msgs::msg::Twist>(   );
           cmd->linear.x = 1.0; // Max is 0.5,   so should threshold
           smoother->sendCommandMsg(  cmd );
          
           // Process velocity smoothing and send updated odometry based on commands
           auto start = smoother->now(   );
           while (  smoother->now(   ) - start < 1.5s ) {
           odom_msg.header.stamp = smoother->now(   );
           if (  linear_vels.size(   ) > 0 ) {
           odom_msg.twist.twist.linear.x = linear_vels.back(   );
           }
           odom_pub->publish(  odom_msg );
           rclcpp::spin_some(  smoother->get_node_base_interface(   ) );
           }
          
           // Sanity check we have the approximately right number of messages for the timespan and timeout
           EXPECT_GT(  linear_vels.size(   ),   19u );
           EXPECT_LT(  linear_vels.size(   ),   30u );
          
           // Should have last command be a stop since we timed out the command stream
           EXPECT_EQ(  linear_vels.back(   ),   0.0 );
          
           // Process to make sure stops at limit in velocity,  
           // doesn't exceed acceleration
           for (  unsigned int i = 0; i != linear_vels.size(   ); i++ ) {
           if (  i > 0 ) {
           double diff = linear_vels[i] - linear_vels[i - 1];
           EXPECT_LT(  diff,   0.126 ); // default accel of 0.5 / 20 hz = 0.125
           }
          
           EXPECT_TRUE(  linear_vels[i] <= 0.5 );
           }
          }
          
     174  TEST(  VelocitySmootherTest,   testfindEtaConstraint )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           rclcpp_lifecycle::State state;
           // default frequency is 20.0
           smoother->configure(  state );
          
           // In range
           EXPECT_EQ(  smoother->findEtaConstraint(  1.0,   1.0,   1.5,   -2.0 ),   -1 );
           EXPECT_EQ(  smoother->findEtaConstraint(  0.5,   0.55,   1.5,   -2.0 ),   -1 );
           EXPECT_EQ(  smoother->findEtaConstraint(  0.5,   0.45,   1.5,   -2.0 ),   -1 );
           // Too high
           EXPECT_EQ(  smoother->findEtaConstraint(  1.0,   2.0,   1.5,   -2.0 ),   0.075 );
           // Too low
           EXPECT_EQ(  smoother->findEtaConstraint(  1.0,   0.0,   1.5,   -2.0 ),   0.1 );
          
           // In a more realistic situation accelerating linear axis
           EXPECT_NEAR(  smoother->findEtaConstraint(  0.40,   0.50,   1.5,   -2.0 ),   0.75,   0.001 );
          }
          
     195  TEST(  VelocitySmootherTest,   testapplyConstraints )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           rclcpp_lifecycle::State state;
           // default frequency is 20.0
           smoother->configure(  state );
           double no_eta = 1.0;
          
           // Apply examples from testfindEtaConstraint
           // In range,   so no eta or acceleration limit impact
           EXPECT_EQ(  smoother->applyConstraints(  1.0,   1.0,   1.5,   -2.0,   no_eta ),   1.0 );
           EXPECT_EQ(  smoother->applyConstraints(  0.5,   0.55,   1.5,   -2.0,   no_eta ),   0.55 );
           EXPECT_EQ(  smoother->applyConstraints(  0.5,   0.45,   1.5,   -2.0,   no_eta ),   0.45 );
           // Too high,   without eta
           EXPECT_NEAR(  smoother->applyConstraints(  1.0,   2.0,   1.5,   -2.0,   no_eta ),   1.075,   0.01 );
           // Too high,   with eta applied on its own axis
           EXPECT_NEAR(  smoother->applyConstraints(  1.0,   2.0,   1.5,   -2.0,   0.075 ),   1.075,   0.01 );
           // On another virtual axis that is OK
           EXPECT_NEAR(  smoother->applyConstraints(  0.5,   0.55,   1.5,   -2.0,   0.075 ),   0.503,   0.01 );
          
           // In a more realistic situation,   applied to angular
           EXPECT_NEAR(  smoother->applyConstraints(  0.8,   1.0,   3.2,   -3.2,   0.75 ),   1.075,   0.95 );
          }
          
     220  TEST(  VelocitySmootherTest,   testCommandCallback )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           rclcpp_lifecycle::State state;
           smoother->configure(  state );
           smoother->activate(  state );
          
           auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>(  "cmd_vel",   1 );
           pub->on_activate(   );
           auto msg = std::make_unique<geometry_msgs::msg::Twist>(   );
           msg->linear.x = 100.0;
           pub->publish(  std::move(  msg ) );
           rclcpp::spin_some(  smoother->get_node_base_interface(   ) );
          
           EXPECT_TRUE(  smoother->hasCommandMsg(   ) );
           EXPECT_EQ(  smoother->lastCommandMsg(   )->linear.x,   100.0 );
          }
          
     239  TEST(  VelocitySmootherTest,   testClosedLoopSub )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           smoother->declare_parameter(  "feedback",   rclcpp::ParameterValue(  std::string(  "OPEN_LOOP" ) ) );
           smoother->set_parameter(  rclcpp::Parameter(  "feedback",   std::string(  "CLOSED_LOOP" ) ) );
           rclcpp_lifecycle::State state;
           smoother->configure(  state );
           EXPECT_TRUE(  smoother->isOdomSmoother(   ) );
          }
          
     250  TEST(  VelocitySmootherTest,   testInvalidParams )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           std::vector<double> max_vels{0.0,   0.0}; // invalid size
           smoother->declare_parameter(  "max_velocity",   rclcpp::ParameterValue(  max_vels ) );
           rclcpp_lifecycle::State state;
           EXPECT_THROW(  smoother->configure(  state ),   std::runtime_error );
          
           smoother->set_parameter(  rclcpp::Parameter(  "feedback",   std::string(  "LAWLS" ) ) );
           EXPECT_THROW(  smoother->configure(  state ),   std::runtime_error );
          }
          
     263  TEST(  VelocitySmootherTest,   testDynamicParameter )
          {
           auto smoother =
           std::make_shared<VelSmootherShim>(   );
           rclcpp_lifecycle::State state;
           smoother->configure(  state );
           smoother->activate(  state );
           EXPECT_FALSE(  smoother->isOdomSmoother(   ) );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           smoother->get_node_base_interface(   ),   smoother->get_node_topics_interface(   ),  
           smoother->get_node_graph_interface(   ),  
           smoother->get_node_services_interface(   ) );
          
           std::vector<double> max_vel{10.0,   10.0,   10.0};
           std::vector<double> min_vel{0.0,   0.0,   0.0};
           std::vector<double> max_accel{10.0,   10.0,   10.0};
           std::vector<double> min_accel{0.0,   0.0,   0.0};
           std::vector<double> deadband{0.0,   0.0,   0.0};
           std::vector<double> bad_test{0.0,   0.0};
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "smoothing_frequency",   100.0 ),  
           rclcpp::Parameter(  "feedback",   std::string(  "CLOSED_LOOP" ) ),  
           rclcpp::Parameter(  "scale_velocities",   true ),  
           rclcpp::Parameter(  "max_velocity",   max_vel ),  
           rclcpp::Parameter(  "min_velocity",   min_vel ),  
           rclcpp::Parameter(  "max_accel",   max_accel ),  
           rclcpp::Parameter(  "max_decel",   min_accel ),  
           rclcpp::Parameter(  "odom_topic",   std::string(  "TEST" ) ),  
           rclcpp::Parameter(  "odom_duration",   2.0 ),  
           rclcpp::Parameter(  "velocity_timeout",   4.0 ),  
           rclcpp::Parameter(  "deadband_velocity",   deadband )} );
          
           rclcpp::spin_until_future_complete(  
           smoother->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  smoother->get_parameter(  "smoothing_frequency" ).as_double(   ),   100.0 );
           EXPECT_EQ(  smoother->get_parameter(  "feedback" ).as_string(   ),   std::string(  "CLOSED_LOOP" ) );
           EXPECT_EQ(  smoother->get_parameter(  "scale_velocities" ).as_bool(   ),   true );
           EXPECT_EQ(  smoother->get_parameter(  "max_velocity" ).as_double_array(   ),   max_vel );
           EXPECT_EQ(  smoother->get_parameter(  "min_velocity" ).as_double_array(   ),   min_vel );
           EXPECT_EQ(  smoother->get_parameter(  "max_accel" ).as_double_array(   ),   max_accel );
           EXPECT_EQ(  smoother->get_parameter(  "max_decel" ).as_double_array(   ),   min_accel );
           EXPECT_EQ(  smoother->get_parameter(  "odom_topic" ).as_string(   ),   std::string(  "TEST" ) );
           EXPECT_EQ(  smoother->get_parameter(  "odom_duration" ).as_double(   ),   2.0 );
           EXPECT_EQ(  smoother->get_parameter(  "velocity_timeout" ).as_double(   ),   4.0 );
           EXPECT_EQ(  smoother->get_parameter(  "deadband_velocity" ).as_double_array(   ),   deadband );
          
           // Test reverting
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "feedback",   std::string(  "OPEN_LOOP" ) )} );
           rclcpp::spin_until_future_complete(  
           smoother->get_node_base_interface(   ),   results );
           EXPECT_EQ(  smoother->get_parameter(  "feedback" ).as_string(   ),   std::string(  "OPEN_LOOP" ) );
          
           // Test invalid change
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "feedback",   std::string(  "LAWLS" ) )} );
           rclcpp::spin_until_future_complete(  smoother->get_node_base_interface(   ),   results );
           EXPECT_FALSE(  results.get(   ).successful );
          
           // Test invalid size
           results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "max_velocity",   bad_test )} );
           rclcpp::spin_until_future_complete(  smoother->get_node_base_interface(   ),   results );
           EXPECT_FALSE(  results.get(   ).successful );
          
           // test full state after major changes
           smoother->deactivate(  state );
           smoother->cleanup(  state );
           smoother->shutdown(  state );
           smoother.reset(   );
          }

navigation2/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp

          /*********************************************************************
           *
           * Software License Agreement (  BSD License )
           *
           * Copyright (  c ) 2008,   Willow Garage,   Inc.
           * All rights reserved.
           *
           * Redistribution and use in source and binary forms,   with or without
           * modification,   are permitted provided that the following conditions
           * are met:
           *
           * * Redistributions of source code must retain the above copyright
           * notice,   this list of conditions and the following disclaimer.
           * * Redistributions in binary form must reproduce the above
           * copyright notice,   this list of conditions and the following
           * disclaimer in the documentation and/or other materials provided
           * with the distribution.
           * * Neither the name of the Willow Garage nor the names of its
           * contributors may be used to endorse or promote products derived
           * from this software without specific prior written permission.
           *
           * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
           * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
           * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
           * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
           * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
           * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
           * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
           * POSSIBILITY OF SUCH DAMAGE.
           *
           * Author: Eitan Marder-Eppstein
           *********************************************************************/
          #ifndef NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
          #define NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
          
          #include <stdio.h>
          #include <string.h>
          #include <stdlib.h>
          #include <stdint.h>
          #include <math.h>
          #include <limits.h>
          #include <algorithm>
          #include "rclcpp/rclcpp.hpp"
          
          /**
           * @class VoxelGrid
           * @brief A 3D grid structure that stores points as an integer array.
           * X and Y index the array and Z selects which bit of the integer
           * is used giving a limit of 16 vertical cells.
           */
          namespace nav2_voxel_grid
          {
          
          enum VoxelStatus
          {
           FREE = 0,  
           UNKNOWN = 1,  
           MARKED = 2,  
          };
          
      65  class VoxelGrid
          {
          public:
           /**
           * @brief Constructor for a voxel grid
           * @param size_x The x size of the grid
           * @param size_y The y size of the grid
           * @param size_z The z size of the grid,   only sizes <= 16 are supported
           */
      74   VoxelGrid(  unsigned int size_x,   unsigned int size_y,   unsigned int size_z );
          
      76   ~VoxelGrid(   );
          
           /**
           * @brief Resizes a voxel grid to the desired size
           * @param size_x The x size of the grid
           * @param size_y The y size of the grid
           * @param size_z The z size of the grid,   only sizes <= 16 are supported
           */
      84   void resize(  unsigned int size_x,   unsigned int size_y,   unsigned int size_z );
          
      86   void reset(   );
      87   uint32_t * getData(   ) {return data_;}
          
      89   inline void markVoxel(  unsigned int x,   unsigned int y,   unsigned int z )
           {
           if (  x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds.\n" );
           return;
           }
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           data_[y * size_x_ + x] |= full_mask; // clear unknown and mark cell
           }
          
      99   inline bool markVoxelInMap(  
           unsigned int x,   unsigned int y,   unsigned int z,  
           unsigned int marked_threshold )
           {
           if (  x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds.\n" );
           return false;
           }
          
           int index = y * size_x_ + x;
           uint32_t * col = &data_[index];
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           *col |= full_mask; // clear unknown and mark cell
          
           unsigned int marked_bits = *col >> 16;
          
           // make sure the number of bits in each is below our thesholds
           return !bitsBelowThreshold(  marked_bits,   marked_threshold );
           }
          
     119   inline void clearVoxel(  unsigned int x,   unsigned int y,   unsigned int z )
           {
           if (  x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds.\n" );
           return;
           }
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           data_[y * size_x_ + x] &= ~(  full_mask ); // clear unknown and clear cell
           }
          
     129   inline void clearVoxelColumn(  unsigned int index )
           {
           assert(  index < size_x_ * size_y_ );
           data_[index] = 0;
           }
          
     135   inline void clearVoxelInMap(  unsigned int x,   unsigned int y,   unsigned int z )
           {
           if (  x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds.\n" );
           return;
           }
           int index = y * size_x_ + x;
           uint32_t * col = &data_[index];
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           *col &= ~(  full_mask ); // clear unknown and clear cell
          
           unsigned int unknown_bits = uint16_t(  *col >> 16 ) ^ uint16_t(  *col );
           unsigned int marked_bits = *col >> 16;
          
           // make sure the number of bits in each is below our thesholds
           if (  bitsBelowThreshold(  unknown_bits,   1 ) && bitsBelowThreshold(  marked_bits,   1 ) ) {
           costmap[index] = 0;
           }
           }
          
     155   inline bool bitsBelowThreshold(  unsigned int n,   unsigned int bit_threshold )
           {
           unsigned int bit_count;
           for (  bit_count = 0; n;  ) {
           ++bit_count;
           if (  bit_count > bit_threshold ) {
           return false;
           }
           n &= n - 1; // clear the least significant bit set
           }
           return true;
           }
          
     168   static inline unsigned int numBits(  unsigned int n )
           {
           unsigned int bit_count;
           for (  bit_count = 0; n; ++bit_count ) {
           n &= n - 1; // clear the least significant bit set
           }
           return bit_count;
           }
          
     177   static VoxelStatus getVoxel(  
           unsigned int x,   unsigned int y,   unsigned int z,  
     179   unsigned int size_x,   unsigned int size_y,   unsigned int size_z,   const uint32_t * data )
           {
           if (  x >= size_x || y >= size_y || z >= size_z ) {
           return UNKNOWN;
           }
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           uint32_t result = data[y * size_x + x] & full_mask;
           unsigned int bits = numBits(  result );
          
           // known marked: 11 = 2 bits,   unknown: 01 = 1 bit,   known free: 00 = 0 bits
           if (  bits < 2 ) {
           if (  bits < 1 ) {
           return FREE;
           }
           return UNKNOWN;
           }
           return MARKED;
           }
          
     198   void markVoxelLine(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,  
           unsigned int max_length = UINT_MAX );
     201   void clearVoxelLine(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,  
           unsigned int max_length = UINT_MAX,   unsigned int min_length = 0 );
     204   void clearVoxelLineInMap(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,   unsigned char * map_2d,  
           unsigned int unknown_threshold,   unsigned int mark_threshold,  
           unsigned char free_cost = 0,   unsigned char unknown_cost = 255,  
           unsigned int max_length = UINT_MAX,   unsigned int min_length = 0 );
          
     210   VoxelStatus getVoxel(  unsigned int x,   unsigned int y,   unsigned int z );
          
           // Are there any obstacles at that (  x,   y ) location in the grid?
     213   VoxelStatus getVoxelColumn(  
           unsigned int x,   unsigned int y,  
           unsigned int unknown_threshold = 0,   unsigned int marked_threshold = 0 );
          
     217   void printVoxelGrid(   );
     218   void printColumnGrid(   );
     219   unsigned int sizeX(   );
     220   unsigned int sizeY(   );
     221   unsigned int sizeZ(   );
          
           template<class ActionType>
           inline void raytraceLine(  
           ActionType at,   double x0,   double y0,   double z0,  
           double x1,   double y1,   double z1,   unsigned int max_length = UINT_MAX,  
           unsigned int min_length = 0 )
           {
           // we need to chose how much to scale our dominant dimension,   based on the
           // maximum length of the line
           double dist = sqrt(  (  x0 - x1 ) * (  x0 - x1 ) + (  y0 - y1 ) * (  y0 - y1 ) + (  z0 - z1 ) * (  z0 - z1 ) );
           if (  (  unsigned int )(  dist ) < min_length ) {
           return;
           }
           double scale,   min_x0,   min_y0,   min_z0;
     236   if (  dist > 0.0 ) {
           scale = std::min(  1.0,   max_length / dist );
          
           // Updating starting point to the point at distance min_length from the initial point
           min_x0 = x0 + (  x1 - x0 ) / dist * min_length;
           min_y0 = y0 + (  y1 - y0 ) / dist * min_length;
           min_z0 = z0 + (  z1 - z0 ) / dist * min_length;
           } else {
           // dist can be 0 if [x0,   y0,   z0]==[x1,   y1,   z1].
           // In this case only this voxel should be processed.
     246   scale = 1.0;
     247   min_x0 = x0;
           min_y0 = y0;
           min_z0 = z0;
           }
          
           int dx = int(  x1 ) - int(  min_x0 ); // NOLINT
           int dy = int(  y1 ) - int(  min_y0 ); // NOLINT
           int dz = int(  z1 ) - int(  min_z0 ); // NOLINT
          
           unsigned int abs_dx = abs(  dx );
           unsigned int abs_dy = abs(  dy );
           unsigned int abs_dz = abs(  dz );
          
           int offset_dx = sign(  dx );
           int offset_dy = sign(  dy ) * size_x_;
           int offset_dz = sign(  dz );
          
           unsigned int z_mask = (  (  1 << 16 ) | 1 ) << (  unsigned int )min_z0;
           unsigned int offset = (  unsigned int )min_y0 * size_x_ + (  unsigned int )min_x0;
          
           GridOffset grid_off(  offset );
           ZOffset z_off(  z_mask );
          
           // is x dominant
           if (  abs_dx >= max(  abs_dy,   abs_dz ) ) {
           int error_y = abs_dx / 2;
           int error_z = abs_dx / 2;
          
           bresenham3D(  
           at,   grid_off,   grid_off,   z_off,   abs_dx,   abs_dy,   abs_dz,   error_y,   error_z,  
           offset_dx,   offset_dy,   offset_dz,   offset,   z_mask,   (  unsigned int )(  scale * abs_dx ) );
           return;
           }
          
           // y is dominant
           if (  abs_dy >= abs_dz ) {
           int error_x = abs_dy / 2;
           int error_z = abs_dy / 2;
          
           bresenham3D(  
           at,   grid_off,   grid_off,   z_off,   abs_dy,   abs_dx,   abs_dz,   error_x,   error_z,  
           offset_dy,   offset_dx,   offset_dz,   offset,   z_mask,   (  unsigned int )(  scale * abs_dy ) );
           return;
           }
          
           // otherwise,   z is dominant
           int error_x = abs_dz / 2;
           int error_y = abs_dz / 2;
          
           bresenham3D(  
           at,   z_off,   grid_off,   grid_off,   abs_dz,   abs_dx,   abs_dy,   error_x,   error_y,   offset_dz,  
           offset_dx,   offset_dy,   offset,   z_mask,   (  unsigned int )(  scale * abs_dz ) );
           }
          
          private:
           // the real work is done here... 3D bresenham implementation
           template<class ActionType,   class OffA,   class OffB,   class OffC>
           inline void bresenham3D(  
           ActionType at,   OffA off_a,   OffB off_b,   OffC off_c,  
           unsigned int abs_da,   unsigned int abs_db,   unsigned int abs_dc,  
           int error_b,   int error_c,   int offset_a,   int offset_b,   int offset_c,   unsigned int & offset,  
           unsigned int & z_mask,   unsigned int max_length = UINT_MAX )
           {
           unsigned int end = std::min(  max_length,   abs_da );
           for (  unsigned int i = 0; i < end; ++i ) {
           at(  offset,   z_mask );
           off_a(  offset_a );
           error_b += abs_db;
           error_c += abs_dc;
           if (  (  unsigned int )error_b >= abs_da ) {
           off_b(  offset_b );
           error_b -= abs_da;
           }
           if (  (  unsigned int )error_c >= abs_da ) {
           off_c(  offset_c );
           error_c -= abs_da;
           }
           }
           at(  offset,   z_mask );
           }
          
           inline int sign(  int i )
           {
           return i > 0 ? 1 : -1;
           }
          
           inline unsigned int max(  unsigned int x,   unsigned int y )
           {
           return x > y ? x : y;
           }
          
           unsigned int size_x_,   size_y_,   size_z_;
           uint32_t * data_;
           unsigned char * costmap;
           rclcpp::Logger logger;
          
           // Aren't functors so much fun... used to recreate the Bresenham macro Eric
           // wrote in the original version,   but in "proper" c++
           class MarkVoxel
           {
          public:
           explicit MarkVoxel(  uint32_t * data )
           : data_(  data ) {}
           inline void operator(   )(  unsigned int offset,   unsigned int z_mask )
           {
           data_[offset] |= z_mask; // clear unknown and mark cell
           }
          
          private:
           uint32_t * data_;
           };
          
           class ClearVoxel
           {
          public:
           explicit ClearVoxel(  uint32_t * data )
           : data_(  data ) {}
           inline void operator(   )(  unsigned int offset,   unsigned int z_mask )
           {
           data_[offset] &= ~(  z_mask ); // clear unknown and clear cell
           }
          
          private:
           uint32_t * data_;
           };
          
           class ClearVoxelInMap
           {
          public:
           ClearVoxelInMap(  
           uint32_t * data,   unsigned char * costmap,  
           unsigned int unknown_clear_threshold,   unsigned int marked_clear_threshold,  
           unsigned char free_cost = 0,   unsigned char unknown_cost = 255 )
           : data_(  data ),   costmap_(  costmap ),  
           unknown_clear_threshold_(  unknown_clear_threshold ),   marked_clear_threshold_(  
           marked_clear_threshold ),  
           free_cost_(  free_cost ),   unknown_cost_(  unknown_cost )
           {
           }
          
           inline void operator(   )(  unsigned int offset,   unsigned int z_mask )
           {
           uint32_t * col = &data_[offset];
           *col &= ~(  z_mask ); // clear unknown and clear cell
          
           unsigned int unknown_bits = uint16_t(  *col >> 16 ) ^ uint16_t(  *col );
           unsigned int marked_bits = *col >> 16;
          
           // make sure the number of bits in each is below our thesholds
           if (  bitsBelowThreshold(  marked_bits,   marked_clear_threshold_ ) ) {
           if (  bitsBelowThreshold(  unknown_bits,   unknown_clear_threshold_ ) ) {
           costmap_[offset] = free_cost_;
           } else {
           costmap_[offset] = unknown_cost_;
           }
           }
           }
          
          private:
           inline bool bitsBelowThreshold(  unsigned int n,   unsigned int bit_threshold )
           {
           unsigned int bit_count;
           for (  bit_count = 0; n;  ) {
           ++bit_count;
           if (  bit_count > bit_threshold ) {
           return false;
           }
           n &= n - 1; // clear the least significant bit set
           }
           return true;
           }
          
           uint32_t * data_;
           unsigned char * costmap_;
           unsigned int unknown_clear_threshold_,   marked_clear_threshold_;
           unsigned char free_cost_,   unknown_cost_;
           };
          
           class GridOffset
           {
          public:
           explicit GridOffset(  unsigned int & offset )
           : offset_(  offset ) {}
           inline void operator(   )(  int offset_val )
           {
           offset_ += offset_val;
           }
          
          private:
           unsigned int & offset_;
           };
          
           class ZOffset
           {
          public:
           explicit ZOffset(  unsigned int & z_mask )
           : z_mask_(  z_mask ) {}
           inline void operator(   )(  int offset_val )
           {
           offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
           }
          
          private:
           unsigned int & z_mask_;
           };
          };
          
          } // namespace nav2_voxel_grid
          
          #endif // NAV2_VOXEL_GRID__VOXEL_GRID_HPP_

navigation2/nav2_voxel_grid/src/voxel_grid.cpp

       1  /*********************************************************************
          *
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2008,   Willow Garage,   Inc.
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of the Willow Garage nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Eitan Marder-Eppstein
          *********************************************************************/
          #include <nav2_voxel_grid/voxel_grid.hpp>
          
          namespace nav2_voxel_grid
          {
      41  VoxelGrid::VoxelGrid(  unsigned int size_x,   unsigned int size_y,   unsigned int size_z )
          : logger(  rclcpp::get_logger(  "voxel_grid" ) )
          {
           size_x_ = size_x;
           size_y_ = size_y;
           size_z_ = size_z;
          
           if (  size_z_ > 16 ) {
           RCLCPP_INFO(  
           logger,   "Error,   this implementation can only support up to 16 z values (  %d )",  
           size_z_ );
           size_z_ = 16;
           }
          
           data_ = new uint32_t[size_x_ * size_y_];
           uint32_t unknown_col = ~(  (  uint32_t )0 ) >> 16;
           uint32_t * col = data_;
           for (  unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
           *col = unknown_col;
           ++col;
           }
          }
          
      64  void VoxelGrid::resize(  unsigned int size_x,   unsigned int size_y,   unsigned int size_z )
          {
           // if we're not actually changing the size,   we can just reset things
           if (  size_x == size_x_ && size_y == size_y_ && size_z == size_z_ ) {
           reset(   );
           return;
           }
          
           delete[] data_;
           size_x_ = size_x;
           size_y_ = size_y;
           size_z_ = size_z;
          
           if (  size_z_ > 16 ) {
           RCLCPP_INFO(  
           logger,   "Error,   this implementation can only support up to 16 z values (  %d )",  
           size_z );
           size_z_ = 16;
           }
          
           data_ = new uint32_t[size_x_ * size_y_];
           uint32_t unknown_col = ~(  (  uint32_t )0 ) >> 16;
           uint32_t * col = data_;
           for (  unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
           *col = unknown_col;
           ++col;
           }
          }
          
      93  VoxelGrid::~VoxelGrid(   )
          {
           delete[] data_;
          }
          
      98  void VoxelGrid::reset(   )
          {
           uint32_t unknown_col = ~(  (  uint32_t )0 ) >> 16;
           uint32_t * col = data_;
           for (  unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
           *col = unknown_col;
           ++col;
           }
          }
          
     108  void VoxelGrid::markVoxelLine(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,  
           unsigned int max_length )
          {
           if (  x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
           z1 >= size_z_ )
           {
           RCLCPP_DEBUG(  
           logger,  
           "Error,   line endpoint out of bounds. "
           "(  %.2f,   %.2f,   %.2f ) to (  %.2f,   %.2f,   %.2f ),   size: (  %d,   %d,   %d )",  
           x0,   y0,   z0,   x1,   y1,   z1,   size_x_,   size_y_,   size_z_ );
           return;
           }
          
           MarkVoxel mv(  data_ );
           raytraceLine(  mv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length );
          }
          
     127  void VoxelGrid::clearVoxelLine(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,  
           unsigned int max_length,   unsigned int min_length )
          {
           if (  x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
           z1 >= size_z_ )
           {
           RCLCPP_DEBUG(  
           logger,  
           "Error,   line endpoint out of bounds. "
           "(  %.2f,   %.2f,   %.2f ) to (  %.2f,   %.2f,   %.2f ),   size: (  %d,   %d,   %d )",  
           x0,   y0,   z0,   x1,   y1,   z1,   size_x_,   size_y_,   size_z_ );
           return;
           }
          
           ClearVoxel cv(  data_ );
           raytraceLine(  cv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
          }
          
     146  void VoxelGrid::clearVoxelLineInMap(  
           double x0,   double y0,   double z0,   double x1,   double y1,   double z1,   unsigned char * map_2d,  
           unsigned int unknown_threshold,   unsigned int mark_threshold,   unsigned char free_cost,  
           unsigned char unknown_cost,   unsigned int max_length,   unsigned int min_length )
          {
           costmap = map_2d;
           if (  map_2d == NULL ) {
           clearVoxelLine(  x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
           return;
           }
          
           if (  x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
           z1 >= size_z_ )
           {
           RCLCPP_DEBUG(  
           logger,  
           "Error,   line endpoint out of bounds. "
           "(  %.2f,   %.2f,   %.2f ) to (  %.2f,   %.2f,   %.2f ),   size: (  %d,   %d,   %d )",  
           x0,   y0,   z0,   x1,   y1,   z1,   size_x_,   size_y_,   size_z_ );
           return;
           }
          
           ClearVoxelInMap cvm(  data_,   costmap,   unknown_threshold,   mark_threshold,   free_cost,   unknown_cost );
           raytraceLine(  cvm,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
          }
          
     172  VoxelStatus VoxelGrid::getVoxel(  unsigned int x,   unsigned int y,   unsigned int z )
          {
           if (  x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds. (  %d,   %d,   %d )\n",   x,   y,   z );
           return UNKNOWN;
           }
           uint32_t full_mask = (  (  uint32_t )1 << z << 16 ) | (  1 << z );
           uint32_t result = data_[y * size_x_ + x] & full_mask;
           unsigned int bits = numBits(  result );
          
           // known marked: 11 = 2 bits,   unknown: 01 = 1 bit,   known free: 00 = 0 bits
           if (  bits < 2 ) {
           if (  bits < 1 ) {
           return FREE;
           }
          
           return UNKNOWN;
           }
          
           return MARKED;
          }
          
     194  VoxelStatus VoxelGrid::getVoxelColumn(  
           unsigned int x,   unsigned int y,  
           unsigned int unknown_threshold,   unsigned int marked_threshold )
          {
           if (  x >= size_x_ || y >= size_y_ ) {
           RCLCPP_DEBUG(  logger,   "Error,   voxel out of bounds. (  %d,   %d )\n",   x,   y );
           return UNKNOWN;
           }
          
           uint32_t * col = &data_[y * size_x_ + x];
          
           unsigned int unknown_bits = uint16_t(  *col >> 16 ) ^ uint16_t(  *col );
           unsigned int marked_bits = *col >> 16;
          
           // check if the number of marked bits qualifies the col as marked
           if (  !bitsBelowThreshold(  marked_bits,   marked_threshold ) ) {
           return MARKED;
           }
          
           // check if the number of unkown bits qualifies the col as unknown
           if (  !bitsBelowThreshold(  unknown_bits,   unknown_threshold ) ) {
           return UNKNOWN;
           }
          
           return FREE;
          }
          
     221  unsigned int VoxelGrid::sizeX(   )
          {
           return size_x_;
          }
          
     226  unsigned int VoxelGrid::sizeY(   )
          {
           return size_y_;
          }
          
     231  unsigned int VoxelGrid::sizeZ(   )
          {
           return size_z_;
          }
          
     236  void VoxelGrid::printVoxelGrid(   )
          {
           for (  unsigned int z = 0; z < size_z_; z++ ) {
           printf(  "Layer z = %u:\n",   z );
           for (  unsigned int y = 0; y < size_y_; y++ ) {
           for (  unsigned int x = 0; x < size_x_; x++ ) {
           printf(  (  getVoxel(  x,   y,   z ) ) == nav2_voxel_grid::MARKED ? "#" : " " );
           }
           printf(  "|\n" );
           }
           }
          }
          
     249  void VoxelGrid::printColumnGrid(   )
          {
           printf(  "Column view:\n" );
           for (  unsigned int y = 0; y < size_y_; y++ ) {
           for (  unsigned int x = 0; x < size_x_; x++ ) {
           printf(  (  getVoxelColumn(  x,   y,   16,   0 ) == nav2_voxel_grid::MARKED ) ? "#" : " " );
           }
           printf(  "|\n" );
           }
          }
          } // namespace nav2_voxel_grid

navigation2/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp

       1  /*********************************************************************
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2021 Samsung Research Russia
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of Willow Garage,   Inc. nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Alexey Merzlyakov
          *********************************************************************/
          #include <nav2_voxel_grid/voxel_grid.hpp>
          #include <gtest/gtest.h>
          
      39  class TestVoxel
          {
          public:
      42   explicit TestVoxel(  uint32_t * data,   int sz_x,   int sz_y )
           : data_(  data )
           {
           size_ = sz_x * sz_y;
           }
      47   inline void operator(   )(  unsigned int off,   unsigned int val )
           {
           ASSERT_TRUE(  off < size_ );
           data_[off] = val;
           }
      52   inline unsigned int operator(   )(  unsigned int off )
           {
           return data_[off];
           }
          
          private:
      58   uint32_t * data_;
           unsigned int size_;
          };
          
      62  TEST(  voxel_grid,   bresenham3DBoundariesCheck )
          {
           const int sz_x = 60;
           const int sz_y = 60;
           const int sz_z = 2;
           const unsigned int max_length = 60;
           const unsigned int min_length = 6;
           nav2_voxel_grid::VoxelGrid vg(  sz_x,   sz_y,   sz_z );
           TestVoxel tv(  vg.getData(   ),   sz_x,   sz_y );
          
           // Initial point - some assymetrically standing point in order to cover most corner cases
           const double x0 = 2.2;
           const double y0 = 3.8;
           const double z0 = 0.4;
           // z-axis won't be domimant
           const double z1 = 0.5;
           // (  x1,   y1 ) point will move
           double x1,   y1;
          
           // Epsilon for outer boundaries of voxel grid array
           const double epsilon = 0.02;
          
           // Running on (  x,   0 ) edge
           y1 = 0.0;
           for (  int i = 0; i <= sz_x; i++ ) {
           if (  i != sz_x ) {
           x1 = i;
           } else {
           x1 = i - epsilon;
           }
           vg.raytraceLine(  tv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
           }
          
           // Running on (  x,   sz_y ) edge
           y1 = sz_y - epsilon;
           for (  int i = 0; i <= sz_x; i++ ) {
           if (  i != sz_x ) {
           x1 = i;
           } else {
           x1 = i - epsilon;
           }
           vg.raytraceLine(  tv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
           }
          
           // Running on (  0,   y ) edge
           x1 = 0.0;
           for (  int j = 0; j <= sz_y; j++ ) {
           if (  j != sz_y ) {
           y1 = j;
           } else {
           y1 = j - epsilon;
           }
           vg.raytraceLine(  tv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
           }
          
           // Running on (  sz_x,   y ) edge
           x1 = sz_x - epsilon;
           for (  int j = 0; j <= sz_y; j++ ) {
           if (  j != sz_y ) {
           y1 = j;
           } else {
           y1 = j - epsilon;
           }
           vg.raytraceLine(  tv,   x0,   y0,   z0,   x1,   y1,   z1,   max_length,   min_length );
           }
          }
          
     129  TEST(  voxel_grid,   bresenham3DSamePoint )
          {
           const int sz_x = 60;
           const int sz_y = 60;
           const int sz_z = 2;
           const unsigned int max_length = 60;
           const unsigned int min_length = 0;
           nav2_voxel_grid::VoxelGrid vg(  sz_x,   sz_y,   sz_z );
           TestVoxel tv(  vg.getData(   ),   sz_x,   sz_y );
          
           // Initial point
           const double x0 = 2.2;
           const double y0 = 3.8;
           const double z0 = 0.4;
          
           unsigned int offset = static_cast<int>(  y0 ) * sz_x + static_cast<int>(  x0 );
           unsigned int val_before = tv(  offset );
           // Same point to check
           vg.raytraceLine(  tv,   x0,   y0,   z0,   x0,   y0,   z0,   max_length,   min_length );
           unsigned int val_after = tv(  offset );
           ASSERT_FALSE(  val_before == val_after );
          }
          
     152  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_voxel_grid/test/voxel_grid_tests.cpp

       1  /*********************************************************************
          *
          * Software License Agreement (  BSD License )
          *
          * Copyright (  c ) 2009,   Willow Garage,   Inc.
          * All rights reserved.
          *
          * Redistribution and use in source and binary forms,   with or without
          * modification,   are permitted provided that the following conditions
          * are met:
          *
          * * Redistributions of source code must retain the above copyright
          * notice,   this list of conditions and the following disclaimer.
          * * Redistributions in binary form must reproduce the above
          * copyright notice,   this list of conditions and the following
          * disclaimer in the documentation and/or other materials provided
          * with the distribution.
          * * Neither the name of Willow Garage,   Inc. nor the names of its
          * contributors may be used to endorse or promote products derived
          * from this software without specific prior written permission.
          *
          * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
          * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT
          * LIMITED TO,   THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
          * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
          * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,   INDIRECT,  
          * INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR CONSEQUENTIAL DAMAGES (  INCLUDING,  
          * BUT NOT LIMITED TO,   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
          * LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
          * CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN CONTRACT,   STRICT
          * LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
          * ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          * POSSIBILITY OF SUCH DAMAGE.
          *
          * Author: Eitan Marder-Eppstein
          *********************************************************************/
          #include <nav2_voxel_grid/voxel_grid.hpp>
          #include <gtest/gtest.h>
          
      40  TEST(  voxel_grid,   basicMarkingAndClearing ) {
           int size_x = 50,   size_y = 10,   size_z = 16;
           nav2_voxel_grid::VoxelGrid vg(  size_x,   size_y,   size_z );
          
           // Put a "tabletop" into the scene. A flat rectangle of set voxels at z = 12.
           int table_z = 12;
           int table_x_min = 5,   table_x_max = 15;
           int table_y_min = 0,   table_y_max = 3;
           for (  int x = table_x_min; x <= table_x_max; x++ ) {
           vg.markVoxelLine(  x,   table_y_min,   table_z,   x,   table_y_max,   table_z );
           }
          
           for (  int i = table_x_min; i <= table_x_max; ++i ) {
           for (  int j = table_y_min; j <= table_y_max; ++j ) {
           // check that each cell of the table is marked
           ASSERT_EQ(  nav2_voxel_grid::MARKED,   vg.getVoxel(  i,   j,   table_z ) );
           }
           }
          
           int mark_count = 0;
           unsigned int unknown_count = 0;
           // go through each cell in the voxel grid and make sure that only 44 are filled in
           for (  unsigned int i = 0; i < vg.sizeX(   ); ++i ) {
           for (  unsigned int j = 0; j < vg.sizeY(   ); ++j ) {
           for (  unsigned int k = 0; k < vg.sizeZ(   ); ++k ) {
           if (  vg.getVoxel(  i,   j,   k ) == nav2_voxel_grid::MARKED ) {
           mark_count++;
           } else if (  vg.getVoxel(  i,   j,   k ) == nav2_voxel_grid::UNKNOWN ) {
           unknown_count++;
           }
           }
           }
           }
           ASSERT_EQ(  mark_count,   44 );
          
           // the rest of the cells should be unknown
           ASSERT_EQ(  unknown_count,   vg.sizeX(   ) * vg.sizeY(   ) * vg.sizeZ(   ) - 44 );
          
           // now,   let's clear one of the rows of the table
           vg.clearVoxelLine(  table_x_min,   table_y_min,   table_z,   table_x_max,   table_y_min,   table_z );
          
           mark_count = 0;
           unknown_count = 0;
           int free_count = 0;
           // go through each cell in the voxel grid and make sure that only 33 are now filled in
           for (  unsigned int i = 0; i < vg.sizeX(   ); ++i ) {
           for (  unsigned int j = 0; j < vg.sizeY(   ); ++j ) {
           for (  unsigned int k = 0; k < vg.sizeZ(   ); ++k ) {
           if (  vg.getVoxel(  i,   j,   k ) == nav2_voxel_grid::MARKED ) {
           mark_count++;
           } else if (  vg.getVoxel(  i,   j,   k ) == nav2_voxel_grid::FREE ) {
           free_count++;
           } else if (  vg.getVoxel(  i,   j,   k ) == nav2_voxel_grid::UNKNOWN ) {
           unknown_count++;
           }
           }
           }
           }
          
           // we've switched 11 cells from marked to free
           ASSERT_EQ(  mark_count,   33 );
          
           // we've just explicitly seen through 11 cells
           ASSERT_EQ(  free_count,   11 );
          
           // the rest of the cells should still be unknown
           ASSERT_EQ(  unknown_count,   vg.sizeX(   ) * vg.sizeY(   ) * vg.sizeZ(   ) - 44 );
          
           // now let's put in a vertical column manually to test markVoxel
           for (  unsigned int i = 0; i < vg.sizeZ(   ); ++i ) {
           vg.markVoxel(  0,   0,   i );
           ASSERT_EQ(  vg.getVoxel(  0,   0,   i ),   nav2_voxel_grid::MARKED );
           }
          
           vg.printColumnGrid(   );
           vg.printVoxelGrid(   );
          
           // now,   let's clear that line of voxels and make sure that they clear out OK
           vg.clearVoxelLine(  0,   0,   0,   0,   0,   vg.sizeZ(   ) - 1 );
          
           for (  unsigned int i = 0; i < vg.sizeZ(   ); ++i ) {
           ASSERT_EQ(  vg.getVoxel(  0,   0,   i ),   nav2_voxel_grid::FREE );
           }
          
           mark_count = 0;
          
           // Visualize the output
           /*
           v->printVoxelGrid(   );
           v->printColumnGrid(   );
          
           printf(  "CostMap:\n===========\n" );
           for(  int y = 0; y < size_y; y++ ){
           for(  int x = 0; x < size_x; x++ ){
           printf(  (  costMap[y * size_x + x] > 0 ? "#" : " " ) );
           }printf(  "|\n" );
           }
           */
          }
          
     140  TEST(  voxel_grid,   InvalidSize ) {
           int size_x = 50,   size_y = 10,   size_z = 17;
           int test_z = 16;
           nav2_voxel_grid::VoxelGrid vg(  size_x,   size_y,   size_z );
           vg.resize(  size_x,   size_y,   test_z );
           vg.resize(  size_x,   size_y,   size_z );
           EXPECT_TRUE(  vg.getVoxelColumn(  51,   10,   0,   0 ) == nav2_voxel_grid::VoxelStatus::UNKNOWN );
           EXPECT_TRUE(  vg.getVoxelColumn(  50,   11,   0,   0 ) == nav2_voxel_grid::VoxelStatus::UNKNOWN );
          }
          
     150  TEST(  voxel_grid,   MarkAndClear ) {
           int size_x = 10,   size_y = 10,   size_z = 10;
           nav2_voxel_grid::VoxelGrid vg(  size_x,   size_y,   size_z );
           vg.markVoxelInMap(  5,   5,   5,   0 );
           EXPECT_EQ(  vg.getVoxel(  5,   5,   5 ),   nav2_voxel_grid::MARKED );
           vg.clearVoxelColumn(  55 );
           EXPECT_EQ(  vg.getVoxel(  5,   5,   5 ),   nav2_voxel_grid::FREE );
          }
          
     159  TEST(  voxel_grid,   clearVoxelLineInMap ) {
           int size_x = 10,   size_y = 10,   size_z = 10;
           nav2_voxel_grid::VoxelGrid vg(  size_x,   size_y,   size_z );
           vg.markVoxelInMap(  0,   0,   5,   0 );
           EXPECT_EQ(  vg.getVoxel(  0,   0,   5 ),   nav2_voxel_grid::MARKED );
          
           unsigned char * map_2d = new unsigned char[100];
           map_2d[0] = 254;
          
           vg.clearVoxelLineInMap(  0,   0,   0,   0,   0,   9,   map_2d,   16,   0 );
          
           EXPECT_EQ(  map_2d[0],   0 );
          
           vg.markVoxelInMap(  0,   0,   5,   0 );
           vg.clearVoxelLineInMap(  0,   0,   0,   0,   0,   9,   nullptr,   16,   0 );
           EXPECT_EQ(  vg.getVoxel(  0,   0,   5 ),   nav2_voxel_grid::FREE );
          
           // Testing for min range for raytrace clearing
           vg.markVoxelInMap(  0,   0,   5,   0 );
           vg.markVoxelInMap(  0,   0,   7,   0 );
           vg.clearVoxelLineInMap(  
           0,   0,   0,   0,   0,   9,   nullptr,   16,   0,   (  unsigned char )'\000',  
           (  unsigned char )'\377',   UINT_MAX,   6 );
           EXPECT_EQ(  vg.getVoxel(  0,   0,   5 ),   nav2_voxel_grid::MARKED );
           EXPECT_EQ(  vg.getVoxel(  0,   0,   7 ),   nav2_voxel_grid::FREE );
          
           delete[] map_2d;
          }
          
     188  TEST(  voxel_grid,   GetVoxelData ) {
           uint32_t * data = new uint32_t[9];
           data[4] = 255;
           data[0] = 0;
           EXPECT_EQ(  
           nav2_voxel_grid::VoxelGrid::getVoxel(  1,   1,   1,   3,   3,   3,   data ),   nav2_voxel_grid::UNKNOWN );
          
           EXPECT_EQ(  
           nav2_voxel_grid::VoxelGrid::getVoxel(  0,   0,   0,   3,   3,   3,   data ),   nav2_voxel_grid::FREE );
           delete[] data;
          }
          
     200  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp

          // Copyright (  c ) 2020 Samsung Research America
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_
          #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_
          #pragma once
          
          #include <string>
          #include <mutex>
          #include <memory>
          
          #include "rclcpp/rclcpp.hpp"
          #include "std_msgs/msg/empty.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_core/waypoint_task_executor.hpp"
          
          namespace nav2_waypoint_follower
          {
          
          /**
           * @brief Simple plugin based on WaypointTaskExecutor,   lets robot to wait for a
           * user input at waypoint arrival.
           */
      34  class InputAtWaypoint : public nav2_core::WaypointTaskExecutor
          {
          public:
          /**
           * @brief Construct a new Input At Waypoint Arrival object
           *
           */
      41   InputAtWaypoint(   );
          
           /**
           * @brief Destroy the Input At Waypoint Arrival object
           *
           */
      47   ~InputAtWaypoint(   );
          
           /**
           * @brief declares and loads parameters used
           * @param parent parent node
           * @param plugin_name name of plugin
           */
      54   void initialize(  
      55   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      56   const std::string & plugin_name );
          
           /**
           * @brief Processor
           * @param curr_pose current pose of the robot
           * @param curr_waypoint_index current waypoint,   that robot just arrived
           * @return if task execution failed
           */
      64   bool processAtWaypoint(  
      65   const geometry_msgs::msg::PoseStamped & curr_pose,   const int & curr_waypoint_index );
          
          protected:
           /**
           * @brief Processor callback
           * @param msg Empty message
           */
      72   void Cb(  const std_msgs::msg::Empty::SharedPtr msg );
          
      74   bool input_received_;
      75   bool is_enabled_;
      76   rclcpp::Duration timeout_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_waypoint_follower" )};
           rclcpp::Clock::SharedPtr clock_;
           std::mutex mutex_;
           rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr subscription_;
          };
          
          } // namespace nav2_waypoint_follower
          
          #endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp

          // Copyright (  c ) 2020 Fetullah Atas
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
          #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
          
          /**
           * While C++17 isn't the project standard. We have to force LLVM/CLang
           * to ignore deprecated declarations
           */
          #define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM
          
          
          #include <experimental/filesystem>
          #include <mutex>
          #include <string>
          #include <exception>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_components/register_node_macro.hpp"
          
          #include "sensor_msgs/msg/image.hpp"
          #include "nav2_core/waypoint_task_executor.hpp"
          #include "opencv4/opencv2/core.hpp"
          #include "opencv4/opencv2/opencv.hpp"
          #include "cv_bridge/cv_bridge.h"
          #include "image_transport/image_transport.hpp"
          
          
          namespace nav2_waypoint_follower
          {
          
      44  class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
          {
          public:
           /**
           * @brief Construct a new Photo At Waypoint object
           *
           */
      51   PhotoAtWaypoint(   );
          
           /**
           * @brief Destroy the Photo At Waypoint object
           *
           */
      57   ~PhotoAtWaypoint(   );
          
           /**
           * @brief declares and loads parameters used
           *
           * @param parent parent node that plugin will be created within
           * @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower
           */
      65   void initialize(  
      66   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      67   const std::string & plugin_name );
          
          
           /**
           * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
           *
           * @param curr_pose current pose of the robot
           * @param curr_waypoint_index current waypoint,   that robot just arrived
           * @return true if task execution was successful
           * @return false if task execution failed
           */
      78   bool processAtWaypoint(  
      79   const geometry_msgs::msg::PoseStamped & curr_pose,   const int & curr_waypoint_index );
          
           /**
           * @brief
           *
           * @param msg
           */
      86   void imageCallback(  const sensor_msgs::msg::Image::SharedPtr msg );
          
           /**
           * @brief given a shared pointer to sensor::msg::Image type,   make a deep copy to inputted cv Mat
           *
           * @param msg
           * @param mat
           */
      94   static void deepCopyMsg2Mat(  const sensor_msgs::msg::Image::SharedPtr & msg,   cv::Mat & mat );
          
          protected:
           // to ensure safety when accessing global var curr_frame_
      98   std::mutex global_mutex_;
           // the taken photos will be saved under this directory
     100   std::experimental::filesystem::path save_dir_;
           // .png ? .jpg ? or some other well known format
     102   std::string image_format_;
           // the topic to subscribe in order capture a frame
     104   std::string image_topic_;
           // whether plugin is enabled
     106   bool is_enabled_;
           // current frame;
     108   sensor_msgs::msg::Image::SharedPtr curr_frame_msg_;
           // global logger
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_waypoint_follower" )};
           // ros susbcriber to get camera image
           rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_image_subscriber_;
          };
          } // namespace nav2_waypoint_follower
          
          #endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp

          // Copyright (  c ) 2020 Fetullah Atas
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
          #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
          #pragma once
          
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "nav2_core/waypoint_task_executor.hpp"
          
          namespace nav2_waypoint_follower
          {
          
          /**
           * @brief Simple plugin based on WaypointTaskExecutor,   lets robot to sleep for a
           * specified amount of time at waypoint arrival. You can reference this class to define
           * your own task and rewrite the body for it.
           *
           */
      34  class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor
          {
          public:
          /**
           * @brief Construct a new Wait At Waypoint Arrival object
           *
           */
      41   WaitAtWaypoint(   );
          
           /**
           * @brief Destroy the Wait At Waypoint Arrival object
           *
           */
      47   ~WaitAtWaypoint(   );
          
           /**
           * @brief declares and loads parameters used (  waypoint_pause_duration_ )
           *
           * @param parent parent node that plugin will be created withing(  waypoint_follower in this case )
           * @param plugin_name
           */
      55   void initialize(  
      56   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      57   const std::string & plugin_name );
          
          
           /**
           * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
           *
           * @param curr_pose current pose of the robot
           * @param curr_waypoint_index current waypoint,   that robot just arrived
           * @return true if task execution was successful
           * @return false if task execution failed
           */
      68   bool processAtWaypoint(  
      69   const geometry_msgs::msg::PoseStamped & curr_pose,   const int & curr_waypoint_index );
          
          protected:
           // the robot will sleep waypoint_pause_duration_ milliseconds
           int waypoint_pause_duration_;
      74   bool is_enabled_;
           rclcpp::Logger logger_{rclcpp::get_logger(  "nav2_waypoint_follower" )};
          };
          
          } // namespace nav2_waypoint_follower
          #endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_

navigation2/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
          #define NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          #include <mutex>
          
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_msgs/action/navigate_to_pose.hpp"
          #include "nav2_msgs/action/follow_waypoints.hpp"
          #include "nav_msgs/msg/path.hpp"
          #include "nav2_util/simple_action_server.hpp"
          #include "rclcpp_action/rclcpp_action.hpp"
          
          #include "nav2_util/node_utils.hpp"
          #include "nav2_core/waypoint_task_executor.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "pluginlib/class_list_macros.hpp"
          
          namespace nav2_waypoint_follower
          {
          
      38  enum class ActionStatus
          {
           UNKNOWN = 0,  
           PROCESSING = 1,  
           FAILED = 2,  
           SUCCEEDED = 3
          };
          
          /**
           * @class nav2_waypoint_follower::WaypointFollower
           * @brief An action server that uses behavior tree for navigating a robot to its
           * goal position.
           */
      51  class WaypointFollower : public nav2_util::LifecycleNode
          {
          public:
           using ActionT = nav2_msgs::action::FollowWaypoints;
           using ClientT = nav2_msgs::action::NavigateToPose;
           using ActionServer = nav2_util::SimpleActionServer<ActionT>;
           using ActionClient = rclcpp_action::Client<ClientT>;
          
           /**
           * @brief A constructor for nav2_waypoint_follower::WaypointFollower class
           * @param options Additional options to control creation of the node.
           */
           explicit WaypointFollower(  const rclcpp::NodeOptions & options = rclcpp::NodeOptions(   ) );
           /**
           * @brief A destructor for nav2_waypoint_follower::WaypointFollower class
           */
           ~WaypointFollower(   );
          
          protected:
           /**
           * @brief Configures member variables
           *
           * Initializes action server for "follow_waypoints"
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_configure(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Activates action server
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_activate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Deactivates action server
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_deactivate(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Resets member variables
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & state ) override;
           /**
           * @brief Called when in shutdown state
           * @param state Reference to LifeCycle node state
           * @return SUCCESS or FAILURE
           */
           nav2_util::CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & state ) override;
          
           /**
           * @brief Action server callbacks
           */
           void followWaypoints(   );
          
           /**
           * @brief Action client result callback
           * @param result Result of action server updated asynchronously
           */
           void resultCallback(  const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result );
          
           /**
           * @brief Action client goal response callback
           * @param goal Response of action server updated asynchronously
           */
           void goalResponseCallback(  const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal );
          
           /**
           * @brief Callback executed when a parameter change is detected
           * @param event ParameterEvent message
           */
           rcl_interfaces::msg::SetParametersResult
           dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters );
          
           // Dynamic parameters handler
           rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
          
           // Our action server
           std::unique_ptr<ActionServer> action_server_;
           ActionClient::SharedPtr nav_to_pose_client_;
           rclcpp::CallbackGroup::SharedPtr callback_group_;
           rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
           std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
           bool stop_on_failure_;
           ActionStatus current_goal_status_;
           int loop_rate_;
           std::vector<int> failed_ids_;
          
           // Task Execution At Waypoint Plugin
           pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor>
           waypoint_task_executor_loader_;
           pluginlib::UniquePtr<nav2_core::WaypointTaskExecutor>
           waypoint_task_executor_;
           std::string waypoint_task_executor_id_;
           std::string waypoint_task_executor_type_;
          };
          
          } // namespace nav2_waypoint_follower
          
          #endif // NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_

navigation2/nav2_waypoint_follower/plugins/input_at_waypoint.cpp

       1  // Copyright (  c ) 2020 Samsung Research America
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
          
          #include <string>
          #include <exception>
          
          #include "pluginlib/class_list_macros.hpp"
          
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_waypoint_follower
          {
          
          using std::placeholders::_1;
          
      28  InputAtWaypoint::InputAtWaypoint(   )
          : input_received_(  false ),  
           is_enabled_(  true ),  
           timeout_(  10.0,   0.0 )
          {
          }
          
      35  InputAtWaypoint::~InputAtWaypoint(   )
          {
          }
          
      39  void InputAtWaypoint::initialize(  
      40   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      41   const std::string & plugin_name )
          {
           auto node = parent.lock(   );
          
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"};
           }
          
           logger_ = node->get_logger(   );
           clock_ = node->get_clock(   );
          
           double timeout;
           std::string input_topic;
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".timeout",  
           rclcpp::ParameterValue(  10.0 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".enabled",  
           rclcpp::ParameterValue(  true ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".input_topic",  
           rclcpp::ParameterValue(  "input_at_waypoint/input" ) );
           node->get_parameter(  plugin_name + ".timeout",   timeout );
           node->get_parameter(  plugin_name + ".enabled",   is_enabled_ );
           node->get_parameter(  plugin_name + ".input_topic",   input_topic );
          
           timeout_ = rclcpp::Duration(  timeout,   0.0 );
          
           RCLCPP_INFO(  
           logger_,   "InputAtWaypoint: Subscribing to input topic %s.",   input_topic.c_str(   ) );
           subscription_ = node->create_subscription<std_msgs::msg::Empty>(  
           input_topic,   1,   std::bind(  &InputAtWaypoint::Cb,   this,   _1 ) );
          }
          
      75  void InputAtWaypoint::Cb(  const std_msgs::msg::Empty::SharedPtr /*msg*/ )
          {
           std::lock_guard<std::mutex> lock(  mutex_ );
           input_received_ = true;
          }
          
      81  bool InputAtWaypoint::processAtWaypoint(  
      82   const geometry_msgs::msg::PoseStamped & /*curr_pose*/,  
           const int & curr_waypoint_index )
          {
           if (  !is_enabled_ ) {
           return true;
           }
          
           input_received_ = false;
          
           rclcpp::Time start = clock_->now(   );
           rclcpp::Rate r(  50 );
           bool input_received = false;
           while (  clock_->now(   ) - start < timeout_ ) {
           {
           std::lock_guard<std::mutex> lock(  mutex_ );
           input_received = input_received_;
           }
          
           if (  input_received ) {
           return true;
           }
          
           r.sleep(   );
           }
          
           RCLCPP_WARN(  
           logger_,   "Unable to get external input at wp %i. Moving on.",   curr_waypoint_index );
           return false;
          }
          
          } // namespace nav2_waypoint_follower
          
     114  PLUGINLIB_EXPORT_CLASS(  
           nav2_waypoint_follower::InputAtWaypoint,  
           nav2_core::WaypointTaskExecutor )

navigation2/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

       1  // Copyright (  c ) 2020 Fetullah Atas
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
          
          #include <string>
          #include <memory>
          
          #include "pluginlib/class_list_macros.hpp"
          
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_waypoint_follower
          {
      26  PhotoAtWaypoint::PhotoAtWaypoint(   )
          {
          }
          
      30  PhotoAtWaypoint::~PhotoAtWaypoint(   )
          {
          }
          
      34  void PhotoAtWaypoint::initialize(  
      35   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      36   const std::string & plugin_name )
          {
           auto node = parent.lock(   );
          
           curr_frame_msg_ = std::make_shared<sensor_msgs::msg::Image>(   );
          
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".enabled",  
           rclcpp::ParameterValue(  true ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".image_topic",  
           rclcpp::ParameterValue(  "/camera/color/image_raw" ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".save_dir",  
           rclcpp::ParameterValue(  "/tmp/waypoint_images" ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,   plugin_name + ".image_format",  
           rclcpp::ParameterValue(  "png" ) );
          
           std::string save_dir_as_string;
           node->get_parameter(  plugin_name + ".enabled",   is_enabled_ );
           node->get_parameter(  plugin_name + ".image_topic",   image_topic_ );
           node->get_parameter(  plugin_name + ".save_dir",   save_dir_as_string );
           node->get_parameter(  plugin_name + ".image_format",   image_format_ );
          
           // get inputted save directory and make sure it exists,   if not log and create it
           save_dir_ = save_dir_as_string;
           try {
           if (  !std::experimental::filesystem::exists(  save_dir_ ) ) {
           RCLCPP_WARN(  
           logger_,  
           "Provided save directory for photo at waypoint plugin does not exist,  "
           "provided directory is: %s,   the directory will be created automatically.",  
           save_dir_.c_str(   )
            );
           if (  !std::experimental::filesystem::create_directory(  save_dir_ ) ) {
           RCLCPP_ERROR(  
           logger_,  
           "Failed to create directory!: %s required by photo at waypoint plugin,   "
           "exiting the plugin with failure!",  
           save_dir_.c_str(   )
            );
           is_enabled_ = false;
           }
           }
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  
           logger_,   "Exception (  %s ) thrown while attempting to create image capture directory."
           " This task executor is being disabled as it cannot save images.",   e.what(   ) );
           is_enabled_ = false;
           }
          
           if (  !is_enabled_ ) {
           RCLCPP_INFO(  
           logger_,   "Photo at waypoint plugin is disabled." );
           } else {
           RCLCPP_INFO(  
           logger_,   "Initializing photo at waypoint plugin,   subscribing to camera topic named; %s",  
           image_topic_.c_str(   ) );
           camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(  
           image_topic_,   rclcpp::SystemDefaultsQoS(   ),  
           std::bind(  &PhotoAtWaypoint::imageCallback,   this,   std::placeholders::_1 ) );
           }
          }
          
     101  bool PhotoAtWaypoint::processAtWaypoint(  
     102   const geometry_msgs::msg::PoseStamped & curr_pose,   const int & curr_waypoint_index )
          {
           if (  !is_enabled_ ) {
           RCLCPP_WARN(  
           logger_,  
           "Photo at waypoint plugin is disabled. Not performing anything"
            );
           return true;
           }
           try {
           // construct the full path to image filename
           std::experimental::filesystem::path file_name = std::to_string(  
           curr_waypoint_index ) + "_" +
           std::to_string(  curr_pose.header.stamp.sec ) + "." + image_format_;
           std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name;
          
           // save the taken photo at this waypoint to given directory
           std::lock_guard<std::mutex> guard(  global_mutex_ );
           cv::Mat curr_frame_mat;
           deepCopyMsg2Mat(  curr_frame_msg_,   curr_frame_mat );
           cv::imwrite(  full_path_image_path.c_str(   ),   curr_frame_mat );
           RCLCPP_INFO(  
           logger_,  
           "Photo has been taken sucessfully at waypoint %i",   curr_waypoint_index );
           } catch (  const std::exception & e ) {
           RCLCPP_ERROR(  
           logger_,  
           "Couldn't take photo at waypoint %i! Caught exception: %s \n"
           "Make sure that the image topic named: %s is valid and active!",  
           curr_waypoint_index,  
           e.what(   ),   image_topic_.c_str(   ) );
           return false;
           }
           return true;
          }
          
     138  void PhotoAtWaypoint::imageCallback(  const sensor_msgs::msg::Image::SharedPtr msg )
          {
           std::lock_guard<std::mutex> guard(  global_mutex_ );
           curr_frame_msg_ = msg;
          }
          
     144  void PhotoAtWaypoint::deepCopyMsg2Mat(  
     145   const sensor_msgs::msg::Image::SharedPtr & msg,  
     146   cv::Mat & mat )
          {
           cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(  msg,   msg->encoding );
           cv::Mat frame = cv_bridge_ptr->image;
           if (  msg->encoding == "rgb8" ) {
           cv::cvtColor(  frame,   frame,   cv::COLOR_RGB2BGR );
           }
           frame.copyTo(  mat );
          }
          
          } // namespace nav2_waypoint_follower
     157  PLUGINLIB_EXPORT_CLASS(  
           nav2_waypoint_follower::PhotoAtWaypoint,  
           nav2_core::WaypointTaskExecutor )

navigation2/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp

       1  // Copyright (  c ) 2020 Fetullah Atas
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
          
          #include <string>
          #include <exception>
          
          #include "pluginlib/class_list_macros.hpp"
          
          #include "nav2_util/node_utils.hpp"
          
          namespace nav2_waypoint_follower
          {
      26  WaitAtWaypoint::WaitAtWaypoint(   )
          : waypoint_pause_duration_(  0 ),  
           is_enabled_(  true )
          {
          }
          
      32  WaitAtWaypoint::~WaitAtWaypoint(   )
          {
          }
          
      36  void WaitAtWaypoint::initialize(  
      37   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,  
      38   const std::string & plugin_name )
          {
           auto node = parent.lock(   );
           if (  !node ) {
           throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"};
           }
           logger_ = node->get_logger(   );
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".waypoint_pause_duration",  
           rclcpp::ParameterValue(  0 ) );
           nav2_util::declare_parameter_if_not_declared(  
           node,  
           plugin_name + ".enabled",  
           rclcpp::ParameterValue(  true ) );
           node->get_parameter(  
           plugin_name + ".waypoint_pause_duration",  
           waypoint_pause_duration_ );
           node->get_parameter(  
           plugin_name + ".enabled",  
           is_enabled_ );
           if (  waypoint_pause_duration_ == 0 ) {
           is_enabled_ = false;
           RCLCPP_INFO(  
           logger_,  
           "Waypoint pause duration is set to zero,   disabling task executor plugin." );
           } else if (  !is_enabled_ ) {
           RCLCPP_INFO(  
           logger_,   "Waypoint task executor plugin is disabled." );
           }
          }
          
      70  bool WaitAtWaypoint::processAtWaypoint(  
      71   const geometry_msgs::msg::PoseStamped & /*curr_pose*/,   const int & curr_waypoint_index )
          {
           if (  !is_enabled_ ) {
           return true;
           }
           RCLCPP_INFO(  
           logger_,   "Arrived at %i'th waypoint,   sleeping for %i milliseconds",  
           curr_waypoint_index,  
           waypoint_pause_duration_ );
           rclcpp::sleep_for(  std::chrono::milliseconds(  waypoint_pause_duration_ ) );
           return true;
          }
          } // namespace nav2_waypoint_follower
      84  PLUGINLIB_EXPORT_CLASS(  
           nav2_waypoint_follower::WaitAtWaypoint,  
           nav2_core::WaypointTaskExecutor )

navigation2/nav2_waypoint_follower/src/main.cpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          
          #include "nav2_waypoint_follower/waypoint_follower.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      20  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           auto node = std::make_shared<nav2_waypoint_follower::WaypointFollower>(   );
           rclcpp::spin(  node->get_node_base_interface(   ) );
           rclcpp::shutdown(   );
          
           return 0;
          }

navigation2/nav2_waypoint_follower/src/waypoint_follower.cpp

       1  // Copyright (  c ) 2019 Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "nav2_waypoint_follower/waypoint_follower.hpp"
          
          #include <fstream>
          #include <memory>
          #include <streambuf>
          #include <string>
          #include <utility>
          #include <vector>
          
          namespace nav2_waypoint_follower
          {
          
          using rcl_interfaces::msg::ParameterType;
          using std::placeholders::_1;
          
      30  WaypointFollower::WaypointFollower(  const rclcpp::NodeOptions & options )
          : nav2_util::LifecycleNode(  "waypoint_follower",   "",   options ),  
           waypoint_task_executor_loader_(  "nav2_waypoint_follower",  
           "nav2_core::WaypointTaskExecutor" )
          {
           RCLCPP_INFO(  get_logger(   ),   "Creating" );
          
           declare_parameter(  "stop_on_failure",   true );
           declare_parameter(  "loop_rate",   20 );
           nav2_util::declare_parameter_if_not_declared(  
           this,   std::string(  "waypoint_task_executor_plugin" ),  
           rclcpp::ParameterValue(  std::string(  "wait_at_waypoint" ) ) );
           nav2_util::declare_parameter_if_not_declared(  
           this,   std::string(  "wait_at_waypoint.plugin" ),  
           rclcpp::ParameterValue(  std::string(  "nav2_waypoint_follower::WaitAtWaypoint" ) ) );
          }
          
      47  WaypointFollower::~WaypointFollower(   )
          {
          }
          
          nav2_util::CallbackReturn
      52  WaypointFollower::on_configure(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring" );
          
           auto node = shared_from_this(   );
          
           stop_on_failure_ = get_parameter(  "stop_on_failure" ).as_bool(   );
           loop_rate_ = get_parameter(  "loop_rate" ).as_int(   );
           waypoint_task_executor_id_ = get_parameter(  "waypoint_task_executor_plugin" ).as_string(   );
          
           callback_group_ = create_callback_group(  
           rclcpp::CallbackGroupType::MutuallyExclusive,  
           false );
           callback_group_executor_.add_callback_group(  callback_group_,   get_node_base_interface(   ) );
          
           nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(  
           get_node_base_interface(   ),  
           get_node_graph_interface(   ),  
           get_node_logging_interface(   ),  
           get_node_waitables_interface(   ),  
           "navigate_to_pose",   callback_group_ );
          
           action_server_ = std::make_unique<ActionServer>(  
           get_node_base_interface(   ),  
           get_node_clock_interface(   ),  
           get_node_logging_interface(   ),  
           get_node_waitables_interface(   ),  
           "follow_waypoints",   std::bind(  &WaypointFollower::followWaypoints,   this ) );
          
           try {
           waypoint_task_executor_type_ = nav2_util::get_plugin_type_param(  
           this,  
           waypoint_task_executor_id_ );
           waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance(  
           waypoint_task_executor_type_ );
           RCLCPP_INFO(  
           get_logger(   ),   "Created waypoint_task_executor : %s of type %s",  
           waypoint_task_executor_id_.c_str(   ),   waypoint_task_executor_type_.c_str(   ) );
           waypoint_task_executor_->initialize(  node,   waypoint_task_executor_id_ );
           } catch (  const pluginlib::PluginlibException & ex ) {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Failed to create waypoint_task_executor. Exception: %s",   ex.what(   ) );
           }
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     101  WaypointFollower::on_activate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Activating" );
          
           action_server_->activate(   );
          
           auto node = shared_from_this(   );
           // Add callback for dynamic parameters
           dyn_params_handler_ = node->add_on_set_parameters_callback(  
           std::bind(  &WaypointFollower::dynamicParametersCallback,   this,   _1 ) );
          
           // create bond connection
           createBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     119  WaypointFollower::on_deactivate(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Deactivating" );
          
           action_server_->deactivate(   );
           dyn_params_handler_.reset(   );
          
           // destroy bond connection
           destroyBond(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     133  WaypointFollower::on_cleanup(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Cleaning up" );
          
           action_server_.reset(   );
           nav_to_pose_client_.reset(   );
          
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          nav2_util::CallbackReturn
     144  WaypointFollower::on_shutdown(  const rclcpp_lifecycle::State & /*state*/ )
          {
           RCLCPP_INFO(  get_logger(   ),   "Shutting down" );
           return nav2_util::CallbackReturn::SUCCESS;
          }
          
          void
     151  WaypointFollower::followWaypoints(   )
          {
           auto goal = action_server_->get_current_goal(   );
           auto feedback = std::make_shared<ActionT::Feedback>(   );
           auto result = std::make_shared<ActionT::Result>(   );
          
           // Check if request is valid
           if (  !action_server_ || !action_server_->is_server_active(   ) ) {
           RCLCPP_DEBUG(  get_logger(   ),   "Action server inactive. Stopping." );
           return;
           }
          
           RCLCPP_INFO(  
           get_logger(   ),   "Received follow waypoint request with %i waypoints.",  
           static_cast<int>(  goal->poses.size(   ) ) );
          
           if (  goal->poses.size(   ) == 0 ) {
           action_server_->succeeded_current(  result );
           return;
           }
          
           rclcpp::WallRate r(  loop_rate_ );
           uint32_t goal_index = 0;
           bool new_goal = true;
          
           while (  rclcpp::ok(   ) ) {
           // Check if asked to stop processing action
           if (  action_server_->is_cancel_requested(   ) ) {
           auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(   );
           callback_group_executor_.spin_until_future_complete(  cancel_future );
           // for result callback processing
           callback_group_executor_.spin_some(   );
           action_server_->terminate_all(   );
           return;
           }
          
           // Check if asked to process another action
           if (  action_server_->is_preempt_requested(   ) ) {
           RCLCPP_INFO(  get_logger(   ),   "Preempting the goal pose." );
           goal = action_server_->accept_pending_goal(   );
           goal_index = 0;
           new_goal = true;
           }
          
           // Check if we need to send a new goal
           if (  new_goal ) {
           new_goal = false;
           ClientT::Goal client_goal;
           client_goal.pose = goal->poses[goal_index];
          
           auto send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions(   );
           send_goal_options.result_callback =
           std::bind(  &WaypointFollower::resultCallback,   this,   std::placeholders::_1 );
           send_goal_options.goal_response_callback =
           std::bind(  &WaypointFollower::goalResponseCallback,   this,   std::placeholders::_1 );
           future_goal_handle_ =
           nav_to_pose_client_->async_send_goal(  client_goal,   send_goal_options );
           current_goal_status_ = ActionStatus::PROCESSING;
           }
          
           feedback->current_waypoint = goal_index;
           action_server_->publish_feedback(  feedback );
          
           if (  current_goal_status_ == ActionStatus::FAILED ) {
           failed_ids_.push_back(  goal_index );
          
           if (  stop_on_failure_ ) {
           RCLCPP_WARN(  
           get_logger(   ),   "Failed to process waypoint %i in waypoint "
           "list and stop on failure is enabled."
           " Terminating action.",   goal_index );
           result->missed_waypoints = failed_ids_;
           action_server_->terminate_current(  result );
           failed_ids_.clear(   );
           return;
           } else {
           RCLCPP_INFO(  
           get_logger(   ),   "Failed to process waypoint %i,  "
           " moving to next.",   goal_index );
           }
           } else if (  current_goal_status_ == ActionStatus::SUCCEEDED ) {
           RCLCPP_INFO(  
           get_logger(   ),   "Succeeded processing waypoint %i,   processing waypoint task execution",  
           goal_index );
           bool is_task_executed = waypoint_task_executor_->processAtWaypoint(  
           goal->poses[goal_index],   goal_index );
           RCLCPP_INFO(  
           get_logger(   ),   "Task execution at waypoint %i %s",   goal_index,  
           is_task_executed ? "succeeded" : "failed!" );
           // if task execution was failed and stop_on_failure_ is on ,   terminate action
           if (  !is_task_executed && stop_on_failure_ ) {
           failed_ids_.push_back(  goal_index );
           RCLCPP_WARN(  
           get_logger(   ),   "Failed to execute task at waypoint %i "
           " stop on failure is enabled."
           " Terminating action.",   goal_index );
           result->missed_waypoints = failed_ids_;
           action_server_->terminate_current(  result );
           failed_ids_.clear(   );
           return;
           } else {
           RCLCPP_INFO(  
           get_logger(   ),   "Handled task execution on waypoint %i,  "
           " moving to next.",   goal_index );
           }
           }
          
           if (  current_goal_status_ != ActionStatus::PROCESSING &&
           current_goal_status_ != ActionStatus::UNKNOWN )
           {
           // Update server state
           goal_index++;
           new_goal = true;
           if (  goal_index >= goal->poses.size(   ) ) {
           RCLCPP_INFO(  
           get_logger(   ),   "Completed all %zu waypoints requested.",  
           goal->poses.size(   ) );
           result->missed_waypoints = failed_ids_;
           action_server_->succeeded_current(  result );
           failed_ids_.clear(   );
           return;
           }
           } else {
           RCLCPP_INFO_EXPRESSION(  
           get_logger(   ),  
           (  static_cast<int>(  now(   ).seconds(   ) ) % 30 == 0 ),  
           "Processing waypoint %i...",   goal_index );
           }
          
           callback_group_executor_.spin_some(   );
           r.sleep(   );
           }
          }
          
          void
     286  WaypointFollower::resultCallback(  
     287   const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result )
          {
           switch (  result.code ) {
           case rclcpp_action::ResultCode::SUCCEEDED:
           current_goal_status_ = ActionStatus::SUCCEEDED;
           return;
           case rclcpp_action::ResultCode::ABORTED:
           current_goal_status_ = ActionStatus::FAILED;
           return;
           case rclcpp_action::ResultCode::CANCELED:
           current_goal_status_ = ActionStatus::FAILED;
           return;
           default:
           current_goal_status_ = ActionStatus::UNKNOWN;
           return;
           }
          }
          
          void
     306  WaypointFollower::goalResponseCallback(  
     307   const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal )
          {
           if (  !goal ) {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "navigate_to_pose action client failed to send goal to server." );
           current_goal_status_ = ActionStatus::FAILED;
           }
          }
          
          rcl_interfaces::msg::SetParametersResult
     318  WaypointFollower::dynamicParametersCallback(  std::vector<rclcpp::Parameter> parameters )
          {
           // No locking required as action server is running on same single threaded executor
           rcl_interfaces::msg::SetParametersResult result;
          
           for (  auto parameter : parameters ) {
           const auto & type = parameter.get_type(   );
           const auto & name = parameter.get_name(   );
          
           if (  type == ParameterType::PARAMETER_INTEGER ) {
           if (  name == "loop_rate" ) {
           loop_rate_ = parameter.as_int(   );
           }
           } else if (  type == ParameterType::PARAMETER_BOOL ) {
           if (  name == "stop_on_failure" ) {
           stop_on_failure_ = parameter.as_bool(   );
           }
           }
           }
          
           result.successful = true;
           return result;
          }
          
          } // namespace nav2_waypoint_follower
          
          #include "rclcpp_components/register_node_macro.hpp"
          
          // Register the component with class_loader.
          // This acts as a sort of entry point,   allowing the component to be discoverable when its library
          // is being loaded into a running process.
     349  RCLCPP_COMPONENTS_REGISTER_NODE(  nav2_waypoint_follower::WaypointFollower )

navigation2/nav2_waypoint_follower/test/test_dynamic_parameters.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gtest/gtest.h"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_waypoint_follower/waypoint_follower.hpp"
          #include "rclcpp/rclcpp.hpp"
          
      26  class WPShim : public nav2_waypoint_follower::WaypointFollower
          {
          public:
      29   WPShim(   )
           : nav2_waypoint_follower::WaypointFollower(  rclcpp::NodeOptions(   ) )
           {
           }
          
      34   void configure(   )
           {
           rclcpp_lifecycle::State state;
           this->on_configure(  state );
           }
          
      40   void activate(   )
           {
           rclcpp_lifecycle::State state;
           this->on_activate(  state );
           }
          };
          
      47  class RclCppFixture
          {
          public:
      50   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      51   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      55  TEST(  WPTest,   test_dynamic_parameters )
          {
           auto follower = std::make_shared<WPShim>(   );
           follower->configure(   );
           follower->activate(   );
          
           auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(  
           follower->get_node_base_interface(   ),   follower->get_node_topics_interface(   ),  
           follower->get_node_graph_interface(   ),  
           follower->get_node_services_interface(   ) );
          
           auto results = rec_param->set_parameters_atomically(  
           {rclcpp::Parameter(  "loop_rate",   100 ),  
           rclcpp::Parameter(  "stop_on_failure",   false )} );
          
           rclcpp::spin_until_future_complete(  
           follower->get_node_base_interface(   ),  
           results );
          
           EXPECT_EQ(  follower->get_parameter(  "loop_rate" ).as_int(   ),   100 );
           EXPECT_EQ(  follower->get_parameter(  "stop_on_failure" ).as_bool(   ),   false );
          }

navigation2/nav2_waypoint_follower/test/test_task_executors.cpp

       1  // Copyright (  c ) 2021,   Samsung Research America
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License. Reserved.
          
          #include <math.h>
          #include <condition_variable>
          #include <memory>
          #include <string>
          #include <vector>
          #include <utility>
          #include <thread>
          
          #include "gtest/gtest.h"
          #include "rclcpp/rclcpp.hpp"
          #include "nav2_util/lifecycle_node.hpp"
          #include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
          #include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
          #include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
          
          
      31  class RclCppFixture
          {
          public:
      34   RclCppFixture(   ) {rclcpp::init(  0,   nullptr );}
      35   ~RclCppFixture(   ) {rclcpp::shutdown(   );}
          };
          RclCppFixture g_rclcppfixture;
          
      39  TEST(  WaypointFollowerTest,   WaitAtWaypoint )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testWaypointNode" );
          
           node->declare_parameter(  "WAW.waypoint_pause_duration",   50 );
          
           std::unique_ptr<nav2_waypoint_follower::WaitAtWaypoint> waw(  
           new nav2_waypoint_follower::WaitAtWaypoint
            );
           waw->initialize(  node,   std::string(  "WAW" ) );
          
           auto start_time = node->now(   );
          
           // should wait 50ms
           geometry_msgs::msg::PoseStamped pose;
           waw->processAtWaypoint(  pose,   0 );
          
           auto end_time = node->now(   );
          
           EXPECT_NEAR(  (  end_time - start_time ).seconds(   ),   0.05,   0.01 );
          
           waw.reset(  new nav2_waypoint_follower::WaitAtWaypoint );
           node->set_parameter(  rclcpp::Parameter(  "WAW.enabled",   false ) );
           waw->initialize(  node,   std::string(  "WAW" ) );
          
           // plugin is not enabled,   should exit
           EXPECT_TRUE(  waw->processAtWaypoint(  pose,   0 ) );
          }
          
      68  TEST(  WaypointFollowerTest,   InputAtWaypoint )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testWaypointNode" );
           auto pub = node->create_publisher<std_msgs::msg::Empty>(  "input_at_waypoint/input",   1 );
           pub->on_activate(   );
           auto publish_message =
           [&,   this](   ) -> void
           {
           rclcpp::Rate(  5 ).sleep(   );
           auto msg = std::make_unique<std_msgs::msg::Empty>(   );
           pub->publish(  std::move(  msg ) );
           rclcpp::spin_some(  node->shared_from_this(   )->get_node_base_interface(   ) );
           };
          
           std::unique_ptr<nav2_waypoint_follower::InputAtWaypoint> iaw(  
           new nav2_waypoint_follower::InputAtWaypoint
            );
           iaw->initialize(  node,   std::string(  "IAW" ) );
          
           auto start_time = node->now(   );
          
           // no input,   should timeout
           geometry_msgs::msg::PoseStamped pose;
           EXPECT_FALSE(  iaw->processAtWaypoint(  pose,   0 ) );
          
           auto end_time = node->now(   );
          
           EXPECT_NEAR(  (  end_time - start_time ).seconds(   ),   10.0,   0.1 );
          
           // has input now,   should work
           std::thread t1(  publish_message );
           EXPECT_TRUE(  iaw->processAtWaypoint(  pose,   0 ) );
           t1.join(   );
          
           iaw.reset(  new nav2_waypoint_follower::InputAtWaypoint );
           node->set_parameter(  rclcpp::Parameter(  "IAW.enabled",   false ) );
           iaw->initialize(  node,   std::string(  "IAW" ) );
          
           // plugin is not enabled,   should exit
           EXPECT_TRUE(  iaw->processAtWaypoint(  pose,   0 ) );
          }
          
     110  TEST(  WaypointFollowerTest,   PhotoAtWaypoint )
          {
           auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  "testWaypointNode" );
           auto pub = node->create_publisher<sensor_msgs::msg::Image>(  "/camera/color/image_raw",   1 );
           pub->on_activate(   );
           std::condition_variable cv;
           std::mutex mtx;
           std::unique_lock<std::mutex> lck(  mtx,   std::defer_lock );
           bool data_published = false;
           auto publish_message =
           [&,   this](   ) -> void
           {
           rclcpp::Rate(  5 ).sleep(   );
           auto msg = std::make_unique<sensor_msgs::msg::Image>(   );
           // fill image msg data.
           msg->encoding = "rgb8";
           msg->height = 240;
           msg->width = 320;
           msg->step = 960;
           auto size = msg->height * msg->width * 3;
           msg->data.reserve(  size );
           int fake_data = 0;
           for (  size_t i = 0; i < size; i++ ) {
           msg->data.push_back(  fake_data++ );
           }
           pub->publish(  std::move(  msg ) );
           rclcpp::spin_some(  node->shared_from_this(   )->get_node_base_interface(   ) );
           lck.lock(   );
           data_published = true;
           cv.notify_one(   );
           lck.unlock(   );
           };
          
           std::unique_ptr<nav2_waypoint_follower::PhotoAtWaypoint> paw(  
           new nav2_waypoint_follower::PhotoAtWaypoint
            );
           paw->initialize(  node,   std::string(  "PAW" ) );
          
           // no images,   throws because can't write
           geometry_msgs::msg::PoseStamped pose;
           EXPECT_FALSE(  paw->processAtWaypoint(  pose,   0 ) );
          
           std::thread t1(  publish_message );
           cv.wait(  lck );
           // has image now,   since we force waiting until image is published
           EXPECT_TRUE(  paw->processAtWaypoint(  pose,   0 ) );
           t1.join(   );
          
           paw.reset(  new nav2_waypoint_follower::PhotoAtWaypoint );
           node->set_parameter(  rclcpp::Parameter(  "PAW.enabled",   false ) );
           paw->initialize(  node,   std::string(  "PAW" ) );
          
           // plugin is not enabled,   should exit
           EXPECT_TRUE(  paw->processAtWaypoint(  pose,   0 ) );
          }

ros2_control/controller_interface/include/controller_interface/chainable_controller_interface.hpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
          #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
          
          #include <vector>
          
          #include "controller_interface/controller_interface_base.hpp"
          #include "controller_interface/visibility_control.h"
          #include "hardware_interface/handle.hpp"
          
          namespace controller_interface
          {
          /// Virtual class to implement when integrating a controller that can be preceded by other
          /// controllers.
          /**
           * Specialization of ControllerInterface class to force implementation of methods specific for
           * "chainable" controller,   i.e.,   controller that can be preceded by an another controller,   for
           * example inner controller of an control cascade.
           *
           */
      34  class ChainableControllerInterface : public ControllerInterfaceBase
          {
          public:
           CONTROLLER_INTERFACE_PUBLIC
      38   ChainableControllerInterface(   );
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual ~ChainableControllerInterface(   ) = default;
          
           CONTROLLER_INTERFACE_PUBLIC
           return_type update(  const rclcpp::Time & time,   const rclcpp::Duration & period ) final;
          
           CONTROLLER_INTERFACE_PUBLIC
           bool is_chainable(   ) const final;
          
           CONTROLLER_INTERFACE_PUBLIC
           std::vector<hardware_interface::CommandInterface> export_reference_interfaces(   ) final;
          
           CONTROLLER_INTERFACE_PUBLIC
           bool set_chained_mode(  bool chained_mode ) final;
          
           CONTROLLER_INTERFACE_PUBLIC
           bool is_in_chained_mode(   ) const final;
          
          protected:
           /// Virtual method that each chainable controller should implement to export its chainable
           /// interfaces.
           /**
           * Each chainable controller implements this methods where all input (  command ) interfaces are
           * exported. The method has the same meaning as `export_command_interface` method from
           * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
           *
           * \returns list of CommandInterfaces that other controller can use as their outputs.
           */
           virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces(   ) = 0;
          
           /// Virtual method that each chainable controller should implement to switch chained mode.
           /**
           * Each chainable controller implements this methods to switch between "chained" and "external"
           * mode. In "chained" mode all external interfaces like subscriber and service servers are
           * disabled to avoid potential concurrency in input commands.
           *
           * \param[in] flag marking a switch to or from chained mode.
           *
           * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode.
           */
           virtual bool on_set_chained_mode(  bool chained_mode );
          
           /// Update reference from input topics when not in chained mode.
           /**
           * Each chainable controller implements this method to update reference from subscribers when not
           * in chained mode.
           *
           * \returns return_type::OK if update is successfully,   otherwise return_type::ERROR.
           */
           virtual return_type update_reference_from_subscribers(   ) = 0;
          
           /// Execute calculations of the controller and update command interfaces.
           /**
           * Update method for chainable controllers.
           * In this method is valid to assume that \reference_interfaces_ hold the values for calculation
           * of the commands in the current control step.
           * This means that this method is called after \update_reference_from_subscribers if controller is
           * not in chained mode.
           *
           * \returns return_type::OK if calculation and writing of interface is successfully,   otherwise
           * return_type::ERROR.
           */
           virtual return_type update_and_write_commands(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Storage of values for reference interfaces
           std::vector<double> reference_interfaces_;
          
          private:
           /// A flag marking if a chainable controller is currently preceded by another controller.
           bool in_chained_mode_ = false;
          };
          
          } // namespace controller_interface
          
          #endif // CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_

ros2_control/controller_interface/include/controller_interface/controller_interface.hpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
          #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface_base.hpp"
          #include "controller_interface/visibility_control.h"
          #include "hardware_interface/handle.hpp"
          
          namespace controller_interface
          {
      28  class ControllerInterface : public controller_interface::ControllerInterfaceBase
          {
          public:
           CONTROLLER_INTERFACE_PUBLIC
      32   ControllerInterface(   );
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual ~ControllerInterface(   ) = default;
          
           /**
           * Controller is not chainable.
           *
           * \returns false.
           */
           CONTROLLER_INTERFACE_PUBLIC
           bool is_chainable(   ) const final;
          
           /**
           * Controller has no reference interfaces.
           *
           * \returns empty list.
           */
           CONTROLLER_INTERFACE_PUBLIC
           std::vector<hardware_interface::CommandInterface> export_reference_interfaces(   ) final;
          
           /**
           * Controller is not chainable,   therefore no chained mode can be set.
           *
           * \returns false.
           */
           CONTROLLER_INTERFACE_PUBLIC
           bool set_chained_mode(  bool chained_mode ) final;
          
           /**
           * Controller can not be in chained mode.
           *
           * \returns false.
           */
           CONTROLLER_INTERFACE_PUBLIC
           bool is_in_chained_mode(   ) const final;
          };
          
          using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;
          
          } // namespace controller_interface
          
          #endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_

ros2_control/controller_interface/include/controller_interface/controller_interface_base.hpp

          // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
          #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/visibility_control.h"
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          
          namespace controller_interface
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      35  enum class return_type : std::uint8_t
          {
           OK = 0,  
           ERROR = 1,  
          };
          
          /// Indicating which interfaces are to be claimed.
          /**
           * One might either claim all available command/state interfaces,  
           * specifying a set of individual interfaces,  
           * or none at all.
           */
      47  enum class interface_configuration_type : std::uint8_t
          {
           ALL = 0,  
           INDIVIDUAL = 1,  
           NONE = 2,  
          };
          
          /// Configuring what command/state interfaces to claim.
          struct InterfaceConfiguration
          {
           interface_configuration_type type;
           std::vector<std::string> names = {};
          };
          
          /**
           * Base interface class for an controller. The interface may not be used to implement a controller.
           * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
           * that should be implemented and extended for a specific controller.
           */
      66  class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
          {
          public:
           CONTROLLER_INTERFACE_PUBLIC
           ControllerInterfaceBase(   ) = default;
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual ~ControllerInterfaceBase(   ) = default;
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual InterfaceConfiguration command_interface_configuration(   ) const = 0;
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual InterfaceConfiguration state_interface_configuration(   ) const = 0;
          
           CONTROLLER_INTERFACE_PUBLIC
           void assign_interfaces(  
           std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,  
           std::vector<hardware_interface::LoanedStateInterface> && state_interfaces );
          
           CONTROLLER_INTERFACE_PUBLIC
           void release_interfaces(   );
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual return_type init(  
           const std::string & controller_name,   const std::string & namespace_ = "",  
           const rclcpp::NodeOptions & node_options =
           rclcpp::NodeOptions(   )
           .allow_undeclared_parameters(  true )
           .automatically_declare_parameters_from_overrides(  true ) );
          
           /// Custom configure method to read additional parameters for controller-nodes
           /*
           * Override default implementation for configure of LifecycleNode to get parameters.
           */
           CONTROLLER_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & configure(   );
          
           /// Extending interface with initialization method which is individual for each controller
           CONTROLLER_INTERFACE_PUBLIC
           virtual CallbackReturn on_init(   ) = 0;
          
           CONTROLLER_INTERFACE_PUBLIC
           virtual return_type update(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           CONTROLLER_INTERFACE_PUBLIC
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node(   );
          
           CONTROLLER_INTERFACE_PUBLIC
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node(   ) const;
          
           CONTROLLER_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & get_state(   ) const;
          
           CONTROLLER_INTERFACE_PUBLIC
           unsigned int get_update_rate(   ) const;
          
           /// Declare and initialize a parameter with a type.
           /**
           *
           * Wrapper function for templated node's declare_parameter(   ) which checks if
           * parameter is already declared.
           * For use in all components that inherit from ControllerInterfaceBase
           */
           template <typename ParameterT>
           auto auto_declare(  const std::string & name,   const ParameterT & default_value )
           {
           if (  !node_->has_parameter(  name ) )
           {
           return node_->declare_parameter<ParameterT>(  name,   default_value );
           }
           else
           {
           return node_->get_parameter(  name ).get_value<ParameterT>(   );
           }
           }
          
           // Methods for chainable controller types with default values so we can put all controllers into
           // one list in Controller Manager
          
           /// Get information if a controller is chainable.
           /**
           * Get information if a controller is chainable.
           *
           * \returns true is controller is chainable and false if it is not.
           */
           CONTROLLER_INTERFACE_PUBLIC
           virtual bool is_chainable(   ) const = 0;
          
           /**
           * Export interfaces for a chainable controller that can be used as command interface of other
           * controllers.
           *
           * \returns list of command interfaces for preceding controllers.
           */
           CONTROLLER_INTERFACE_PUBLIC
           virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces(   ) = 0;
          
           /**
           * Set chained mode of a chainable controller. This method triggers internal processes to switch
           * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode
           * usually involves disabling of subscribers and other external interfaces to avoid potential
           * concurrency in input commands.
           *
           * \returns true if mode is switched successfully and false if not.
           */
           CONTROLLER_INTERFACE_PUBLIC
           virtual bool set_chained_mode(  bool chained_mode ) = 0;
          
           /// Get information if a controller is currently in chained mode.
           /**
           * Get information about controller if it is currently used in chained mode. In chained mode only
           * internal interfaces are available and all subscribers are expected to be disabled. This
           * prevents concurrent writing to controller's inputs from multiple sources.
           *
           * \returns true is controller is in chained mode and false if it is not.
           */
           CONTROLLER_INTERFACE_PUBLIC
           virtual bool is_in_chained_mode(   ) const = 0;
          
          protected:
           std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
           std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
           unsigned int update_rate_ = 0;
          
          private:
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
          };
          
          using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
          
          } // namespace controller_interface
          
          #endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_

ros2_control/controller_interface/include/controller_interface/helpers.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_INTERFACE__HELPERS_HPP_
          #define CONTROLLER_INTERFACE__HELPERS_HPP_
          
          #include <functional>
          #include <string>
          #include <vector>
          
          namespace controller_interface
          {
          /// Reorder interfaces with references according to joint names or full interface names.
          /**
           * Method to reorder and check if all expected interfaces are provided for the joint.
           * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in
           * `ordered_names`.
           *
           * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces.
           * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces.
           * The valued inputs are list of joint names or interface full names.
           * If joint names are used for ordering,   \p interface_type specifies valid interface.
           * If full interface names are used for ordering,   \p interface_type should be empty string (  "" ).
           * \param[in] interface_type used for ordering interfaces with respect to joint names.
           * \param[out] ordered_interfaces vector with ordered interfaces.
           * \return true if all interfaces or joints in \p ordered_names are found,   otherwise false.
           */
          template <typename T>
      40  bool get_ordered_interfaces(  
           std::vector<T> & unordered_interfaces,   const std::vector<std::string> & ordered_names,  
           const std::string & interface_type,   std::vector<std::reference_wrapper<T>> & ordered_interfaces )
          {
           ordered_interfaces.reserve(  ordered_names.size(   ) );
           for (  const auto & name : ordered_names )
           {
           for (  auto & interface : unordered_interfaces )
           {
           if (  !interface_type.empty(   ) )
           {
           // check case where:
           // (  <joint> == <joint> AND <interface> == <interface> ) OR <joint>/<interface> == 'full name'
           if (  
           (  (  name == interface.get_prefix_name(   ) ) &&
           (  interface_type == interface.get_interface_name(   ) ) ) ||
           (  (  name + "/" + interface_type ) == interface.get_name(   ) ) )
           {
           ordered_interfaces.push_back(  std::ref(  interface ) );
           }
           }
           else
           {
           if (  name == interface.get_name(   ) )
           {
           ordered_interfaces.push_back(  std::ref(  interface ) );
           }
           }
           }
           }
          
           return ordered_names.size(   ) == ordered_interfaces.size(   );
          }
          
      74  inline bool interface_list_contains_interface_type(  
           const std::vector<std::string> & interface_type_list,   const std::string & interface_type )
          {
           return std::find(  interface_type_list.begin(   ),   interface_type_list.end(   ),   interface_type ) !=
           interface_type_list.end(   );
          }
          
          } // namespace controller_interface
          
          #endif // CONTROLLER_INTERFACE__HELPERS_HPP_

ros2_control/controller_interface/include/controller_interface/visibility_control.h

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
          #define CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define CONTROLLER_INTERFACE_EXPORT __attribute__(  (  dllexport ) )
          #define CONTROLLER_INTERFACE_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define CONTROLLER_INTERFACE_EXPORT __declspec(  dllexport )
          #define CONTROLLER_INTERFACE_IMPORT __declspec(  dllimport )
          #endif
          #ifdef CONTROLLER_INTERFACE_BUILDING_DLL
          #define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_EXPORT
          #else
          #define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_IMPORT
          #endif
          #define CONTROLLER_INTERFACE_PUBLIC_TYPE CONTROLLER_INTERFACE_PUBLIC
          #define CONTROLLER_INTERFACE_LOCAL
          #else
          #define CONTROLLER_INTERFACE_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define CONTROLLER_INTERFACE_IMPORT
          #if __GNUC__ >= 4
          #define CONTROLLER_INTERFACE_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define CONTROLLER_INTERFACE_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define CONTROLLER_INTERFACE_PUBLIC
          #define CONTROLLER_INTERFACE_LOCAL
          #endif
          #define CONTROLLER_INTERFACE_PUBLIC_TYPE
          #endif
          
          #endif // CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_

ros2_control/controller_interface/include/semantic_components/force_torque_sensor.hpp

          // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
          #define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
          
          #include <limits>
          #include <string>
          #include <vector>
          
          #include "geometry_msgs/msg/wrench.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "semantic_components/semantic_component_interface.hpp"
          
          namespace semantic_components
          {
      28  class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
          {
          public:
           /// Constructor for "standard" 6D FTS
      32   explicit ForceTorqueSensor(  const std::string & name ) : SemanticComponentInterface(  name,   6 )
           {
           // If 6D FTS use standard names
           interface_names_.emplace_back(  name_ + "/" + "force.x" );
           interface_names_.emplace_back(  name_ + "/" + "force.y" );
           interface_names_.emplace_back(  name_ + "/" + "force.z" );
           interface_names_.emplace_back(  name_ + "/" + "torque.x" );
           interface_names_.emplace_back(  name_ + "/" + "torque.y" );
           interface_names_.emplace_back(  name_ + "/" + "torque.z" );
          
           // Set all interfaces existing
           std::fill(  existing_axes_.begin(   ),   existing_axes_.end(   ),   true );
          
           // Set default force and torque values to NaN
           std::fill(  forces_.begin(   ),   forces_.end(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           std::fill(  torques_.begin(   ),   torques_.end(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           }
          
           /// Constructor for 6D FTS with custom interface names.
           /**
           * Constructor for 6D FTS with custom interface names or FTS with less then six measurement axes,  
           * e.g.,   1D and 2D force load cells.
           * For non existing axes interface is empty string,   i.e.,   (  "" );
           *
           * The name should be in the following order:
           * force X,   force Y,   force Z,   torque X,   torque Y,   torque Z.
           */
      59   ForceTorqueSensor(  
      60   const std::string & interface_force_x,   const std::string & interface_force_y,  
      61   const std::string & interface_force_z,   const std::string & interface_torque_x,  
      62   const std::string & interface_torque_y,   const std::string & interface_torque_z )
           : SemanticComponentInterface(  "",   6 )
           {
           auto check_and_add_interface = [this](  const std::string & interface_name,   const int index )
           {
           if (  !interface_name.empty(   ) )
           {
           interface_names_.emplace_back(  interface_name );
           existing_axes_[index] = true;
           }
           else
           {
           existing_axes_[index] = false;
           }
           };
          
           check_and_add_interface(  interface_force_x,   0 );
           check_and_add_interface(  interface_force_y,   1 );
           check_and_add_interface(  interface_force_z,   2 );
           check_and_add_interface(  interface_torque_x,   3 );
           check_and_add_interface(  interface_torque_y,   4 );
           check_and_add_interface(  interface_torque_z,   5 );
          
           // Set default force and torque values to NaN
           std::fill(  forces_.begin(   ),   forces_.end(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           std::fill(  torques_.begin(   ),   torques_.end(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           }
          
           virtual ~ForceTorqueSensor(   ) = default;
          
           /// Return forces.
           /**
           * Return forces of a FTS.
           *
           * \return array of size 3 with force values.
           */
           std::array<double,   3> get_forces(   )
           {
           size_t interface_counter = 0;
           for (  size_t i = 0; i < 3; ++i )
           {
           if (  existing_axes_[i] )
           {
           forces_[i] = state_interfaces_[interface_counter].get(   ).get_value(   );
           ++interface_counter;
           }
           }
           return forces_;
           }
          
           /// Return torque.
           /**
           * Return torques of a FTS.
           *
           * \return array of size 3 with torque values.
           */
     118   std::array<double,   3> get_torques(   )
           {
           // find out how many force interfaces are being used
           // torque interfaces will be found from the next index onward
           auto torque_interface_counter =
           std::count(  existing_axes_.begin(   ),   existing_axes_.begin(   ) + 3,   true );
          
           for (  size_t i = 3; i < 6; ++i )
           {
           if (  existing_axes_[i] )
           {
           torques_[i - 3] = state_interfaces_[torque_interface_counter].get(   ).get_value(   );
           ++torque_interface_counter;
           }
           }
           return torques_;
           }
          
           /// Return Wrench message with forces and torques.
           /**
           * Constructs and return a wrench message from the current values.
           * The method assumes that the interface names on the construction are in the following order:
           * force X,   force Y,   force Z,   torque X,   torque Y,   torque Z.
           *
           * \return wrench message from values;
           */
     144   bool get_values_as_message(  geometry_msgs::msg::Wrench & message )
           {
           // call get_forces(   ) and get_troque(   ) to update with the latest values
           get_forces(   );
           get_torques(   );
          
           // update the message values
           message.force.x = forces_[0];
           message.force.y = forces_[1];
           message.force.z = forces_[2];
           message.torque.x = torques_[0];
           message.torque.y = torques_[1];
           message.torque.z = torques_[2];
          
           return true;
           }
          
          protected:
           /// Vector with existing axes for sensors with less then 6D axes.
           // Order is: force X,   force Y,   force Z,   torque X,   torque Y,   torque Z.
           std::array<bool,   6> existing_axes_;
           std::array<double,   3> forces_;
           std::array<double,   3> torques_;
          };
          
          } // namespace semantic_components
          
          #endif // SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_

ros2_control/controller_interface/include/semantic_components/imu_sensor.hpp

          // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
          #define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
          
          #include <limits>
          #include <string>
          #include <vector>
          
          #include "semantic_components/semantic_component_interface.hpp"
          #include "sensor_msgs/msg/imu.hpp"
          
          namespace semantic_components
          {
      27  class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
          {
          public:
      30   explicit IMUSensor(  const std::string & name ) : SemanticComponentInterface(  name,   10 )
           {
           interface_names_.emplace_back(  name_ + "/" + "orientation.x" );
           interface_names_.emplace_back(  name_ + "/" + "orientation.y" );
           interface_names_.emplace_back(  name_ + "/" + "orientation.z" );
           interface_names_.emplace_back(  name_ + "/" + "orientation.w" );
           interface_names_.emplace_back(  name_ + "/" + "angular_velocity.x" );
           interface_names_.emplace_back(  name_ + "/" + "angular_velocity.y" );
           interface_names_.emplace_back(  name_ + "/" + "angular_velocity.z" );
           interface_names_.emplace_back(  name_ + "/" + "linear_acceleration.x" );
           interface_names_.emplace_back(  name_ + "/" + "linear_acceleration.y" );
           interface_names_.emplace_back(  name_ + "/" + "linear_acceleration.z" );
          
           // Set default values to NaN
           orientation_.fill(  std::numeric_limits<double>::quiet_NaN(   ) );
           angular_velocity_.fill(  std::numeric_limits<double>::quiet_NaN(   ) );
           linear_acceleration_.fill(  std::numeric_limits<double>::quiet_NaN(   ) );
           }
          
           virtual ~IMUSensor(   ) = default;
          
           /// Return orientation.
           /**
           * Return orientation reported by an IMU
           *
           * \return array of size 4 with orientation quaternion (  x,  y,  z,  w )
           */
           std::array<double,   4> get_orientation(   )
           {
           size_t interface_offset = 0;
           for (  size_t i = 0; i < orientation_.size(   ); ++i )
           {
           orientation_[i] = state_interfaces_[interface_offset + i].get(   ).get_value(   );
           }
           return orientation_;
           }
          
           /// Return angular velocity.
           /**
           * Return angular velocity reported by an IMU
           *
           * \return array of size 3 with angular velocity values.
           */
      73   std::array<double,   3> get_angular_velocity(   )
           {
           size_t interface_offset = orientation_.size(   );
           for (  size_t i = 0; i < angular_velocity_.size(   ); ++i )
           {
           angular_velocity_[i] = state_interfaces_[interface_offset + i].get(   ).get_value(   );
           }
           return angular_velocity_;
           }
          
           /// Return linear acceleration.
           /**
           * Return linear acceleration reported by an IMU
           *
           * \return array of size 3 with linear acceleration values.
           */
      89   std::array<double,   3> get_linear_acceleration(   )
           {
           size_t interface_offset = orientation_.size(   ) + angular_velocity_.size(   );
           for (  size_t i = 0; i < linear_acceleration_.size(   ); ++i )
           {
           linear_acceleration_[i] = state_interfaces_[interface_offset + i].get(   ).get_value(   );
           }
           return linear_acceleration_;
           }
          
           /// Return Imu message with orientation,   angular velocity and linear acceleration
           /**
           * Constructs and return a IMU message from the current values.
           * \return imu message from values;
           */
     104   bool get_values_as_message(  sensor_msgs::msg::Imu & message )
           {
           // call get_orientation(   ) and get_angular_velocity(   ) get_linear_acceleration(   ) to
           // update with the latest values
           get_orientation(   );
           get_angular_velocity(   );
           get_linear_acceleration(   );
          
           // update the message values,   covariances unknown
           message.orientation.x = orientation_[0];
           message.orientation.y = orientation_[1];
           message.orientation.z = orientation_[2];
           message.orientation.w = orientation_[3];
           message.orientation_covariance.fill(  .0 );
          
           message.angular_velocity.x = angular_velocity_[0];
           message.angular_velocity.y = angular_velocity_[1];
           message.angular_velocity.z = angular_velocity_[2];
           message.angular_velocity_covariance.fill(  .0 );
          
           message.linear_acceleration.x = linear_acceleration_[0];
           message.linear_acceleration.y = linear_acceleration_[1];
           message.linear_acceleration.z = linear_acceleration_[2];
           message.linear_acceleration_covariance.fill(  .0 );
          
           return true;
           }
          
          protected:
           // Order is: orientation X,  Y,  Z,  W angular velocity X,  Y,  Z and linear acceleration X,  Y,  Z
           std::array<double,   4> orientation_;
           std::array<double,   3> angular_velocity_;
           std::array<double,   3> linear_acceleration_;
          };
          
          } // namespace semantic_components
          
          #endif // SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_

ros2_control/controller_interface/include/semantic_components/semantic_component_interface.hpp

          // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_
          #define SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_
          
          #include <string>
          #include <vector>
          
          #include "controller_interface/helpers.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          
          namespace semantic_components
          {
          template <typename MessageReturnType>
      27  class SemanticComponentInterface
          {
          public:
      30   explicit SemanticComponentInterface(  const std::string & name,   size_t size = 0 )
           {
           name_ = name;
           interface_names_.reserve(  size );
           state_interfaces_.reserve(  size );
           }
          
           ~SemanticComponentInterface(   ) = default;
          
           /// Assign loaned state interfaces from the hardware.
           /**
           * Assign loaned state interfaces on the controller start.
           *
           * \param[in] state_interfaces vector of interfaces provided by the controller.
           */
           bool assign_loaned_state_interfaces(  
           std::vector<hardware_interface::LoanedStateInterface> & state_interfaces )
           {
           return controller_interface::get_ordered_interfaces(  
           state_interfaces,   interface_names_,   "",   state_interfaces_ );
           }
          
           /// Release loaned interfaces from the hardware.
           void release_interfaces(   ) { state_interfaces_.clear(   ); }
          
           /// Definition of state interface names for the component.
           /**
           * The function should be used in "state_interface_configuration(   )" of a controller to provide
           * standardized interface names semantic component.
           *
           * \default Default implementation defined state interfaces as "name/NR" where NR is number
           * from 0 to size of values;
           * \return list of strings with state interface names for the semantic component.
           */
      64   virtual std::vector<std::string> get_state_interface_names(   )
           {
           if (  interface_names_.empty(   ) )
           {
           for (  auto i = 0u; i < interface_names_.capacity(   ); ++i )
           {
           interface_names_.emplace_back(  name_ + "/" + std::to_string(  i + 1 ) );
           }
           }
           return interface_names_;
           }
          
           /// Return all values.
           /**
           * \return true if it gets all the values,   else false
           */
      80   bool get_values(  std::vector<double> & values ) const
           {
           // check we have sufficient memory
           if (  values.capacity(   ) != state_interfaces_.size(   ) )
           {
           return false;
           }
           // insert all the values
           for (  size_t i = 0; i < state_interfaces_.size(   ); ++i )
           {
           values.emplace_back(  state_interfaces_[i].get(   ).get_value(   ) );
           }
           return true;
           }
          
           /// Return values as MessageReturnType
           /**
           * \return false by default
           */
      99   bool get_values_as_message(  MessageReturnType & /* message */ ) { return false; }
          
          protected:
           std::string name_;
           std::vector<std::string> interface_names_;
           std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> state_interfaces_;
          };
          
          } // namespace semantic_components
          
          #endif // SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_

ros2_control/controller_interface/src/chainable_controller_interface.cpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "controller_interface/chainable_controller_interface.hpp"
          
          #include <vector>
          
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace controller_interface
          {
      24  ChainableControllerInterface::ChainableControllerInterface(   ) : ControllerInterfaceBase(   ) {}
          
      26  bool ChainableControllerInterface::is_chainable(   ) const { return true; }
          
      28  return_type ChainableControllerInterface::update(  
      29   const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type ret = return_type::ERROR;
          
           if (  !is_in_chained_mode(   ) )
           {
           ret = update_reference_from_subscribers(   );
           if (  ret != return_type::OK )
           {
           return ret;
           }
           }
          
           ret = update_and_write_commands(  time,   period );
          
           return ret;
          }
          
          std::vector<hardware_interface::CommandInterface>
      48  ChainableControllerInterface::export_reference_interfaces(   )
          {
           auto reference_interfaces = on_export_reference_interfaces(   );
          
           // check if the "reference_interfaces_" variable is resized to number of interfaces
           if (  reference_interfaces_.size(   ) != reference_interfaces.size(   ) )
           {
           // TODO(  destogl ): Should here be "FATAL"? It is fatal in terms of controller but not for the
           // framework
           RCLCPP_FATAL(  
           get_node(   )->get_logger(   ),  
           "The internal storage for reference values 'reference_interfaces_' variable has size '%zu',   "
           "but it is expected to have the size '%zu' equal to the number of exported reference "
           "interfaces. No reference interface will be exported. Please correct and recompile "
           "the controller with name '%s' and try again.",  
           reference_interfaces_.size(   ),   reference_interfaces.size(   ),   get_node(   )->get_name(   ) );
           reference_interfaces.clear(   );
           }
          
           // check if the names of the reference interfaces begin with the controller's name
           for (  const auto & interface : reference_interfaces )
           {
           if (  interface.get_prefix_name(   ) != get_node(   )->get_name(   ) )
           {
           RCLCPP_FATAL(  
           get_node(   )->get_logger(   ),  
           "The name of the interface '%s' does not begin with the controller's name. This is "
           "mandatory "
           " for reference interfaces. No reference interface will be exported. Please correct and "
           "recompile the controller with name '%s' and try again.",  
           interface.get_name(   ).c_str(   ),   get_node(   )->get_name(   ) );
           reference_interfaces.clear(   );
           break;
           }
           }
          
           return reference_interfaces;
          }
          
      87  bool ChainableControllerInterface::set_chained_mode(  bool chained_mode )
          {
           bool result = false;
          
           if (  get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = on_set_chained_mode(  chained_mode );
          
           if (  result )
           {
           in_chained_mode_ = chained_mode;
           }
           }
           else
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "Can not change controller's chained mode because it is no in '%s' state. "
           "Current state is '%s'.",  
           hardware_interface::lifecycle_state_names::UNCONFIGURED,   get_state(   ).label(   ).c_str(   ) );
           }
          
           return result;
          }
          
     112  bool ChainableControllerInterface::is_in_chained_mode(   ) const { return in_chained_mode_; }
          
     114  bool ChainableControllerInterface::on_set_chained_mode(  bool /*chained_mode*/ ) { return true; }
          
          } // namespace controller_interface

ros2_control/controller_interface/src/controller_interface.cpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "controller_interface/controller_interface.hpp"
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace controller_interface
          {
      27  ControllerInterface::ControllerInterface(   ) : ControllerInterfaceBase(   ) {}
          
      29  bool ControllerInterface::is_chainable(   ) const { return false; }
          
      31  std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces(   )
          {
           return {};
          }
          
      36  bool ControllerInterface::set_chained_mode(  bool /*chained_mode*/ ) { return false; }
          
      38  bool ControllerInterface::is_in_chained_mode(   ) const { return false; }
          
          } // namespace controller_interface

ros2_control/controller_interface/src/controller_interface_base.cpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "controller_interface/controller_interface_base.hpp"
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace controller_interface
          {
      27  return_type ControllerInterfaceBase::init(  
      28   const std::string & controller_name,   const std::string & namespace_,  
      29   const rclcpp::NodeOptions & node_options )
          {
           node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(  
           controller_name,   namespace_,   node_options,   false ); // disable LifecycleNode service interfaces
          
           try
           {
           auto_declare<int>(  "update_rate",   0 );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return return_type::ERROR;
           }
          
           switch (  on_init(   ) )
           {
           case LifecycleNodeInterface::CallbackReturn::SUCCESS:
           break;
           case LifecycleNodeInterface::CallbackReturn::ERROR:
           case LifecycleNodeInterface::CallbackReturn::FAILURE:
           return return_type::ERROR;
           }
          
           node_->register_on_configure(  
           std::bind(  &ControllerInterfaceBase::on_configure,   this,   std::placeholders::_1 ) );
          
           node_->register_on_cleanup(  
           std::bind(  &ControllerInterfaceBase::on_cleanup,   this,   std::placeholders::_1 ) );
          
           node_->register_on_activate(  
           std::bind(  &ControllerInterfaceBase::on_activate,   this,   std::placeholders::_1 ) );
          
           node_->register_on_deactivate(  
           std::bind(  &ControllerInterfaceBase::on_deactivate,   this,   std::placeholders::_1 ) );
          
           node_->register_on_shutdown(  
           std::bind(  &ControllerInterfaceBase::on_shutdown,   this,   std::placeholders::_1 ) );
          
           node_->register_on_error(  
           std::bind(  &ControllerInterfaceBase::on_error,   this,   std::placeholders::_1 ) );
          
           return return_type::OK;
          }
          
      74  const rclcpp_lifecycle::State & ControllerInterfaceBase::configure(   )
          {
           // TODO(  destogl ): this should actually happen in "on_configure" but I am not sure how to get
           // overrides correctly in combination with std::bind. The goal is to have the following calls:
           // 1. CM: controller.get_node(   )->configure(   )
           // 2. LifecycleNode: ControllerInterfaceBase::on_configure(   )
           // 3. ControllerInterfaceBase: <controller>::on_configure(   )
           // Then we don't need to do state-machine related checks.
           //
           // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
           if (  get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
           {
           update_rate_ = get_node(   )->get_parameter(  "update_rate" ).as_int(   );
           }
          
           return get_node(   )->configure(   );
          }
          
      92  void ControllerInterfaceBase::assign_interfaces(  
           std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,  
           std::vector<hardware_interface::LoanedStateInterface> && state_interfaces )
          {
           command_interfaces_ = std::forward<decltype(  command_interfaces )>(  command_interfaces );
           state_interfaces_ = std::forward<decltype(  state_interfaces )>(  state_interfaces );
          }
          
     100  void ControllerInterfaceBase::release_interfaces(   )
          {
           command_interfaces_.clear(   );
           state_interfaces_.clear(   );
          }
          
     106  const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state(   ) const
          {
           return node_->get_current_state(   );
          }
          
     111  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node(   )
          {
           if (  !node_.get(   ) )
           {
           throw std::runtime_error(  "Lifecycle node hasn't been initialized yet!" );
           }
           return node_;
          }
          
     120  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node(   ) const
          {
           if (  !node_.get(   ) )
           {
           throw std::runtime_error(  "Lifecycle node hasn't been initialized yet!" );
           }
           return node_;
          }
          
     129  unsigned int ControllerInterfaceBase::get_update_rate(   ) const { return update_rate_; }
          
          } // namespace controller_interface

ros2_control/controller_interface/test/test_chainable_controller_interface.cpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_chainable_controller_interface.hpp"
          
          #include <gmock/gmock.h>
          
      19  TEST_F(  ChainableControllerInterfaceTest,   default_returns )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           EXPECT_TRUE(  controller.is_chainable(   ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          }
          
      31  TEST_F(  ChainableControllerInterfaceTest,   export_reference_interfaces )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           auto reference_interfaces = controller.export_reference_interfaces(   );
          
           ASSERT_EQ(  reference_interfaces.size(   ),   1u );
           EXPECT_EQ(  reference_interfaces[0].get_prefix_name(   ),   TEST_CONTROLLER_NAME );
           EXPECT_EQ(  reference_interfaces[0].get_interface_name(   ),   "test_itf" );
          
           EXPECT_EQ(  reference_interfaces[0].get_value(   ),   INTERFACE_VALUE );
          }
          
      48  TEST_F(  ChainableControllerInterfaceTest,   reference_interfaces_storage_not_correct_size )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           // expect empty return because storage is not resized
           controller.reference_interfaces_.clear(   );
           auto reference_interfaces = controller.export_reference_interfaces(   );
           ASSERT_TRUE(  reference_interfaces.empty(   ) );
          }
          
      62  TEST_F(  ChainableControllerInterfaceTest,   reference_interfaces_prefix_is_not_node_name )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           controller.set_name_prefix_of_reference_interfaces(  "some_not_correct_interface_prefix" );
          
           // expect empty return because interface prefix is not equal to the node name
           auto reference_interfaces = controller.export_reference_interfaces(   );
           ASSERT_TRUE(  reference_interfaces.empty(   ) );
          }
          
      77  TEST_F(  ChainableControllerInterfaceTest,   setting_chained_mode )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           auto reference_interfaces = controller.export_reference_interfaces(   );
           ASSERT_EQ(  reference_interfaces.size(   ),   1u );
          
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           // Fail setting chained mode
           EXPECT_EQ(  reference_interfaces[0].get_value(   ),   INTERFACE_VALUE );
          
           EXPECT_FALSE(  controller.set_chained_mode(  true ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           EXPECT_FALSE(  controller.set_chained_mode(  false ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           // Success setting chained mode
           reference_interfaces[0].set_value(  0.0 );
          
           EXPECT_TRUE(  controller.set_chained_mode(  true ) );
           EXPECT_TRUE(  controller.is_in_chained_mode(   ) );
          
           controller.configure(   );
           EXPECT_TRUE(  controller.set_chained_mode(  false ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           controller.get_node(   )->activate(   );
           // Can not change chained mode until in "ACTIVE" state
           EXPECT_FALSE(  controller.set_chained_mode(  true ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           controller.get_node(   )->deactivate(   );
           EXPECT_TRUE(  controller.set_chained_mode(  true ) );
           EXPECT_TRUE(  controller.is_in_chained_mode(   ) );
          
           // Can change 'chained' mode only in "UNCONFIGURED" state
           controller.get_node(   )->cleanup(   );
           EXPECT_TRUE(  controller.set_chained_mode(  false ) );
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          }
          
     124  TEST_F(  ChainableControllerInterfaceTest,   test_update_logic )
          {
           TestableChainableControllerInterface controller;
          
           // initialize,   create node
           ASSERT_EQ(  controller.init(  TEST_CONTROLLER_NAME ),   controller_interface::return_type::OK );
           ASSERT_NO_THROW(  controller.get_node(   ) );
          
           EXPECT_FALSE(  controller.is_in_chained_mode(   ) );
          
           // call update and update it from subscriber because not in chained mode
           ASSERT_EQ(  
           controller.update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
           ASSERT_EQ(  controller.reference_interfaces_[0],   INTERFACE_VALUE_INITIAL_REF - 1 );
          
           // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec.
           controller.set_new_reference_interface_value(  INTERFACE_VALUE_SUBSCRIBER_ERROR );
           ASSERT_EQ(  
           controller.update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
           ASSERT_EQ(  controller.reference_interfaces_[0],   INTERFACE_VALUE_INITIAL_REF - 1 );
          
           // Provoke error from update - return ERROR,   but reference interface is updated and not reduced
           controller.set_new_reference_interface_value(  INTERFACE_VALUE_UPDATE_ERROR );
           ASSERT_EQ(  
           controller.update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
           ASSERT_EQ(  controller.reference_interfaces_[0],   INTERFACE_VALUE_UPDATE_ERROR );
          
           controller.reference_interfaces_[0] = 0.0;
          
           EXPECT_TRUE(  controller.set_chained_mode(  true ) );
           EXPECT_TRUE(  controller.is_in_chained_mode(   ) );
          
           // Provoke error in update from subscribers - return OK because update of subscribers is not used
           // reference interface is not updated (  updated directly because in chained mode )
           controller.set_new_reference_interface_value(  INTERFACE_VALUE_SUBSCRIBER_ERROR );
           ASSERT_EQ(  
           controller.update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
           ASSERT_EQ(  controller.reference_interfaces_[0],   -1.0 );
          
           // Provoke error from update - return ERROR,   but reference interface is updated directly
           controller.set_new_reference_interface_value(  INTERFACE_VALUE_SUBSCRIBER_ERROR );
           controller.reference_interfaces_[0] = INTERFACE_VALUE_UPDATE_ERROR;
           ASSERT_EQ(  
           controller.update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
           ASSERT_EQ(  controller.reference_interfaces_[0],   INTERFACE_VALUE_UPDATE_ERROR );
          }

ros2_control/controller_interface/test/test_chainable_controller_interface.hpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
          #define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
          
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "controller_interface/chainable_controller_interface.hpp"
          #include "hardware_interface/handle.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
      27  constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller";
      28  constexpr double INTERFACE_VALUE = 1989.0;
          constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0;
      30  constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0;
      31  constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0;
          
      33  class TestableChainableControllerInterface
          : public controller_interface::ChainableControllerInterface
          {
          public:
           FRIEND_TEST(  ChainableControllerInterfaceTest,   reference_interfaces_storage_not_correct_size );
           FRIEND_TEST(  ChainableControllerInterfaceTest,   test_update_logic );
          
           TestableChainableControllerInterface(   )
           {
           reference_interfaces_.reserve(  1 );
           reference_interfaces_.push_back(  INTERFACE_VALUE );
           }
          
           controller_interface::CallbackReturn on_init(   ) override
           {
           // set default value
           name_prefix_of_reference_interfaces_ = get_node(   )->get_name(   );
          
           return controller_interface::CallbackReturn::SUCCESS;
           }
          
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           // Implementation of ChainableController virtual methods
           std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces(   ) override
           {
           std::vector<hardware_interface::CommandInterface> command_interfaces;
          
           command_interfaces.push_back(  hardware_interface::CommandInterface(  
           name_prefix_of_reference_interfaces_,   "test_itf",   &reference_interfaces_[0] ) );
          
           return command_interfaces;
           }
          
           bool on_set_chained_mode(  bool /*chained_mode*/ ) override
           {
           if (  reference_interfaces_[0] == 0.0 )
           {
           return true;
           }
           else
           {
           return false;
           }
           }
          
           controller_interface::return_type update_reference_from_subscribers(   ) override
           {
           if (  reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR )
           {
           return controller_interface::return_type::ERROR;
           }
          
           reference_interfaces_[0] = reference_interface_value_;
           return controller_interface::return_type::OK;
           }
          
           controller_interface::return_type update_and_write_commands(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           if (  reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR )
           {
           return controller_interface::return_type::ERROR;
           }
          
           reference_interfaces_[0] -= 1;
          
           return controller_interface::return_type::OK;
           }
          
           void set_name_prefix_of_reference_interfaces(  const std::string & prefix )
           {
           name_prefix_of_reference_interfaces_ = prefix;
           }
          
           void set_new_reference_interface_value(  const double ref_itf_value )
           {
           reference_interface_value_ = ref_itf_value;
           }
          
           std::string name_prefix_of_reference_interfaces_;
           double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF;
          };
          
     127  class ChainableControllerInterfaceTest : public ::testing::Test
          {
          public:
     130   static void SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
     132   static void TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          };
          
          #endif // TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_

ros2_control/controller_interface/test/test_controller_interface.hpp

          // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CONTROLLER_INTERFACE_HPP_
          #define TEST_CONTROLLER_INTERFACE_HPP_
          
          #include "controller_interface/controller_interface.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
      21  constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface";
          
      23  class TestableControllerInterface : public controller_interface::ControllerInterface
          {
          public:
      26   controller_interface::CallbackReturn on_init(   ) override
           {
           return controller_interface::CallbackReturn::SUCCESS;
           }
          
      31   controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           controller_interface::return_type update(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return controller_interface::return_type::OK;
           }
          };
          
          class TestableControllerInterfaceInitError : public TestableControllerInterface
          {
          public:
           controller_interface::CallbackReturn on_init(   ) override
           {
           return controller_interface::CallbackReturn::ERROR;
           }
          };
          
          class TestableControllerInterfaceInitFailure : public TestableControllerInterface
          {
          public:
           controller_interface::CallbackReturn on_init(   ) override
           {
           return controller_interface::CallbackReturn::FAILURE;
           }
          };
          
          #endif // TEST_CONTROLLER_INTERFACE_HPP_

ros2_control/controller_interface/test/test_controller_with_options.cpp

          // Copyright 2021 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_controller_with_options.hpp"
          
          #include <gtest/gtest.h>
          #include <string>
          
          #include "rclcpp/rclcpp.hpp"
          
      22  class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
          {
      24   FRIEND_TEST(  ControllerWithOption,   init_with_overrides );
      25   FRIEND_TEST(  ControllerWithOption,   init_without_overrides );
          };
          
          template <class T,   size_t N>
          constexpr size_t arrlen(  T (  & )[N] )
          {
           return N;
          }
          
          TEST(  ControllerWithOption,   init_with_overrides )
          {
           // mocks the declaration of overrides parameters in a yaml file
           char const * const argv[] = {"",   "--ros-args",  
           "-p",   "parameter_list.parameter1:=1.",  
           "-p",   "parameter_list.parameter2:=2.",  
           "-p",   "parameter_list.parameter3:=3."};
           int argc = arrlen(  argv );
           rclcpp::init(  argc,   argv );
           // creates the controller
           FriendControllerWithOptions controller;
           EXPECT_EQ(  controller.init(  "controller_name" ),   controller_interface::return_type::OK );
           // checks that the node options have been updated
           const auto & node_options = controller.get_node(   )->get_node_options(   );
           EXPECT_TRUE(  node_options.allow_undeclared_parameters(   ) );
           EXPECT_TRUE(  node_options.automatically_declare_parameters_from_overrides(   ) );
           // checks that the parameters have been correctly processed
           EXPECT_EQ(  controller.params.size(   ),   3u );
           EXPECT_EQ(  controller.params["parameter1"],   1. );
           EXPECT_EQ(  controller.params["parameter2"],   2. );
           EXPECT_EQ(  controller.params["parameter3"],   3. );
           rclcpp::shutdown(   );
          }
          
          TEST(  ControllerWithOption,   init_without_overrides )
          {
           // mocks the declaration of overrides parameters in a yaml file
           char const * const argv[] = {""};
           int argc = arrlen(  argv );
           rclcpp::init(  argc,   argv );
           // creates the controller
           FriendControllerWithOptions controller;
           EXPECT_EQ(  controller.init(  "controller_name" ),   controller_interface::return_type::ERROR );
           // checks that the node options have been updated
           const auto & node_options = controller.get_node(   )->get_node_options(   );
           EXPECT_TRUE(  node_options.allow_undeclared_parameters(   ) );
           EXPECT_TRUE(  node_options.automatically_declare_parameters_from_overrides(   ) );
           // checks that no parameter has been declared from overrides
           EXPECT_EQ(  controller.params.size(   ),   0u );
           rclcpp::shutdown(   );
          }

ros2_control/controller_interface/test/test_controller_with_options.hpp

          // Copyright 2021 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CONTROLLER_WITH_OPTIONS_HPP_
          #define TEST_CONTROLLER_WITH_OPTIONS_HPP_
          
          #include <map>
          #include <memory>
          #include <string>
          
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          
          namespace controller_with_options
          {
          /**
           * Example of Controller using the ControllerInterface::init(  const std::string &,  
           * rclcpp::NodeOptions & ) function. In this example,   we set the node options so that parameters
           * are automatically declared by overrides. This is a rare use case,   but it can be useful in some
           * situations.
           */
      33  class ControllerWithOptions : public controller_interface::ControllerInterface
          {
          public:
           ControllerWithOptions(   ) = default;
           LifecycleNodeInterface::CallbackReturn on_init(   ) override
           {
           return LifecycleNodeInterface::CallbackReturn::SUCCESS;
           }
          
      42   controller_interface::return_type init(  
           const std::string & controller_name,   const std::string & namespace_ = "",  
           const rclcpp::NodeOptions & node_options =
           rclcpp::NodeOptions(   )
           .allow_undeclared_parameters(  true )
           .automatically_declare_parameters_from_overrides(  true ) ) override
           {
           ControllerInterface::init(  controller_name,   namespace_,   node_options );
          
           switch (  on_init(   ) )
           {
           case LifecycleNodeInterface::CallbackReturn::SUCCESS:
           break;
           case LifecycleNodeInterface::CallbackReturn::ERROR:
           case LifecycleNodeInterface::CallbackReturn::FAILURE:
           return controller_interface::return_type::ERROR;
           }
           if (  get_node(   )->get_parameters(  "parameter_list",   params ) )
           {
           RCLCPP_INFO_STREAM(  get_node(   )->get_logger(   ),   "I found " << params.size(   ) << " parameters." );
           return controller_interface::return_type::OK;
           }
           else
           {
           return controller_interface::return_type::ERROR;
           }
           }
          
      70   controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
      76   controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           controller_interface::return_type update(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return controller_interface::return_type::OK;
           }
          
           std::map<std::string,   double> params;
          };
          } // namespace controller_with_options
          
          #endif // TEST_CONTROLLER_WITH_OPTIONS_HPP_

ros2_control/controller_interface/test/test_force_torque_sensor.cpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #include "test_force_torque_sensor.hpp"
          
          #include <cmath>
          #include <memory>
          #include <string>
          #include <vector>
          
      26  void ForceTorqueSensorTest::TearDown(   ) { force_torque_sensor_.reset(  nullptr ); }
          
      28  TEST_F(  ForceTorqueSensorTest,   validate_all_with_default_names )
          {
           // create the force torque sensor
           force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>(  sensor_name_ );
          
           // validate the component name
           ASSERT_EQ(  force_torque_sensor_->name_,   sensor_name_ );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  force_torque_sensor_->interface_names_.size(   ),   size_ );
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the default interface_names_
           ASSERT_TRUE(  std::equal(  
           force_torque_sensor_->interface_names_.begin(   ),   force_torque_sensor_->interface_names_.end(   ),  
           full_interface_names_.begin(   ),   full_interface_names_.end(   ) ) );
          
           // get the interface names
           std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names(   );
          
           // assign values to force
           hardware_interface::StateInterface force_x{
           sensor_name_,   fts_interface_names_[0],   &force_values_[0]};
           hardware_interface::StateInterface force_y{
           sensor_name_,   fts_interface_names_[1],   &force_values_[1]};
           hardware_interface::StateInterface force_z{
           sensor_name_,   fts_interface_names_[2],   &force_values_[2]};
          
           // assign values to torque
           hardware_interface::StateInterface torque_x{
           sensor_name_,   fts_interface_names_[3],   &torque_values_[0]};
           hardware_interface::StateInterface torque_y{
           sensor_name_,   fts_interface_names_[4],   &torque_values_[1]};
           hardware_interface::StateInterface torque_z{
           sensor_name_,   fts_interface_names_[5],   &torque_values_[2]};
          
           // create local state interface vector
           std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
           temp_state_interfaces.reserve(  6 );
          
           // insert the interfaces in jumbled sequence
           temp_state_interfaces.emplace_back(  torque_y );
           temp_state_interfaces.emplace_back(  force_z );
           temp_state_interfaces.emplace_back(  force_x );
           temp_state_interfaces.emplace_back(  torque_z );
           temp_state_interfaces.emplace_back(  torque_x );
           temp_state_interfaces.emplace_back(  force_y );
          
           // now call the function to make them in order like interface_names
           force_torque_sensor_->assign_loaned_state_interfaces(  temp_state_interfaces );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   size_ );
          
           // validate the force values
           std::array<double,   3> temp_force_values = force_torque_sensor_->get_forces(   );
           ASSERT_EQ(  temp_force_values,   force_values_ );
          
           // validate the torque values
           std::array<double,   3> temp_torque_values = force_torque_sensor_->get_torques(   );
           ASSERT_EQ(  temp_torque_values,   torque_values_ );
          
           // validate get_values_as_message
           geometry_msgs::msg::Wrench temp_message;
           ASSERT_TRUE(  force_torque_sensor_->get_values_as_message(  temp_message ) );
           ASSERT_EQ(  temp_message.force.x,   force_values_[0] );
           ASSERT_EQ(  temp_message.force.y,   force_values_[1] );
           ASSERT_EQ(  temp_message.force.z,   force_values_[2] );
           ASSERT_EQ(  temp_message.torque.x,   torque_values_[0] );
           ASSERT_EQ(  temp_message.torque.y,   torque_values_[1] );
           ASSERT_EQ(  temp_message.torque.z,   torque_values_[2] );
          
           // release the state_interfaces_
           force_torque_sensor_->release_interfaces(   );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   0u );
          }
          
     108  TEST_F(  ForceTorqueSensorTest,   validate_all_with_custom_names )
          {
           // create the force torque sensor with force.x,   force.z and torque.y,   torque.z
           force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>(  
           sensor_name_ + "/" + "force.x",   "",   sensor_name_ + "/" + "force.z",   "",  
           sensor_name_ + "/" + "torque.y",   sensor_name_ + "/" + "torque.z" );
          
           // validate the component name
           // it should be "" as we specified the interface's names explicitly
           ASSERT_EQ(  force_torque_sensor_->name_,   "" );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  force_torque_sensor_->interface_names_.size(   ),   4u );
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the default interface_names_
           ASSERT_EQ(  force_torque_sensor_->interface_names_[0],   full_interface_names_[0] );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[1],   full_interface_names_[2] );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[2],   full_interface_names_[4] );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[3],   full_interface_names_[5] );
          
           // get the interface names
           std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names(   );
          
           // assign values to force.x and force.z
           hardware_interface::StateInterface force_x{
           sensor_name_,   fts_interface_names_[0],   &force_values_[0]};
           hardware_interface::StateInterface force_z{
           sensor_name_,   fts_interface_names_[2],   &force_values_[2]};
          
           // assign values to torque.y and torque.z
           hardware_interface::StateInterface torque_y{
           sensor_name_,   fts_interface_names_[4],   &torque_values_[1]};
           hardware_interface::StateInterface torque_z{
           sensor_name_,   fts_interface_names_[5],   &torque_values_[2]};
          
           // create local state interface vector
           std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
           temp_state_interfaces.reserve(  4 );
          
           // insert the interfaces in jumbled sequence
           temp_state_interfaces.emplace_back(  torque_y );
           temp_state_interfaces.emplace_back(  force_z );
           temp_state_interfaces.emplace_back(  force_x );
           temp_state_interfaces.emplace_back(  torque_z );
          
           // now call the function to make them in order like interface_names
           force_torque_sensor_->assign_loaned_state_interfaces(  temp_state_interfaces );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   4u );
          
           // validate the force values
           std::array<double,   3> temp_force_values = force_torque_sensor_->get_forces(   );
           ASSERT_EQ(  temp_force_values[0],   force_values_[0] );
           ASSERT_TRUE(  std::isnan(  temp_force_values[1] ) );
           ASSERT_EQ(  temp_force_values[2],   force_values_[2] );
          
           // validate the torque values
           std::array<double,   3> temp_torque_values = force_torque_sensor_->get_torques(   );
           ASSERT_TRUE(  std::isnan(  temp_torque_values[0] ) );
           ASSERT_EQ(  temp_torque_values[1],   torque_values_[1] );
           ASSERT_EQ(  temp_torque_values[2],   torque_values_[2] );
          
           // validate get_values_as_message
           geometry_msgs::msg::Wrench temp_message;
           ASSERT_TRUE(  force_torque_sensor_->get_values_as_message(  temp_message ) );
           ASSERT_EQ(  temp_message.force.x,   force_values_[0] );
           ASSERT_TRUE(  std::isnan(  temp_message.force.y ) );
           ASSERT_EQ(  temp_message.force.z,   force_values_[2] );
           ASSERT_TRUE(  std::isnan(  temp_message.torque.x ) );
           ASSERT_EQ(  temp_message.torque.y,   torque_values_[1] );
           ASSERT_EQ(  temp_message.torque.z,   torque_values_[2] );
          
           // release the state_interfaces_
           force_torque_sensor_->release_interfaces(   );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   0u );
          }
          
     190  TEST_F(  ForceTorqueSensorTest,   validate_all_custom_names )
          {
           // create the force torque sensor with force.x,   force.z and torque.y,   torque.z
           force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>(  
           sensor_name_ + "/" + "force.x",   sensor_name_ + "/" + "force.y",   sensor_name_ + "/" + "force.z",  
           sensor_name_ + "/" + "torque.x",   sensor_name_ + "/" + "torque.y",  
           sensor_name_ + "/" + "torque.z" );
          
           // validate the component name
           // it should be "" as we specified the interface's names explicitly
           ASSERT_EQ(  force_torque_sensor_->name_,   "" );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  force_torque_sensor_->interface_names_.size(   ),   size_ );
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the custom interface_names_
           ASSERT_EQ(  force_torque_sensor_->interface_names_[0],   sensor_name_ + "/" + "force.x" );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[1],   sensor_name_ + "/" + "force.y" );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[2],   sensor_name_ + "/" + "force.z" );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[3],   sensor_name_ + "/" + "torque.x" );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[4],   sensor_name_ + "/" + "torque.y" );
           ASSERT_EQ(  force_torque_sensor_->interface_names_[5],   sensor_name_ + "/" + "torque.z" );
          
           // assign values to force
           hardware_interface::StateInterface force_x{
           sensor_name_,   fts_interface_names_[0],   &force_values_[0]};
           hardware_interface::StateInterface force_y{
           sensor_name_,   fts_interface_names_[1],   &force_values_[1]};
           hardware_interface::StateInterface force_z{
           sensor_name_,   fts_interface_names_[2],   &force_values_[2]};
          
           // assign values to torque
           hardware_interface::StateInterface torque_x{
           sensor_name_,   fts_interface_names_[3],   &torque_values_[0]};
           hardware_interface::StateInterface torque_y{
           sensor_name_,   fts_interface_names_[4],   &torque_values_[1]};
           hardware_interface::StateInterface torque_z{
           sensor_name_,   fts_interface_names_[5],   &torque_values_[2]};
          
           // create local state interface vector
           std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
          
           // insert the interfaces in jumbled sequence
           temp_state_interfaces.emplace_back(  torque_y );
           temp_state_interfaces.emplace_back(  force_z );
           temp_state_interfaces.emplace_back(  force_x );
           temp_state_interfaces.emplace_back(  torque_z );
           temp_state_interfaces.emplace_back(  torque_x );
           temp_state_interfaces.emplace_back(  force_y );
          
           // now call the function to make them in order like interface_names
           force_torque_sensor_->assign_loaned_state_interfaces(  temp_state_interfaces );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   size_ );
          
           // validate the force values
           std::array<double,   3> temp_force_values = force_torque_sensor_->get_forces(   );
           ASSERT_EQ(  temp_force_values[0],   force_values_[0] );
           ASSERT_EQ(  temp_force_values[1],   force_values_[1] );
           ASSERT_EQ(  temp_force_values[2],   force_values_[2] );
          
           // validate the torque values
           std::array<double,   3> temp_torque_values = force_torque_sensor_->get_torques(   );
           ASSERT_EQ(  temp_torque_values[0],   torque_values_[0] );
           ASSERT_EQ(  temp_torque_values[1],   torque_values_[1] );
           ASSERT_EQ(  temp_torque_values[2],   torque_values_[2] );
          
           // validate get_values_as_message
           geometry_msgs::msg::Wrench temp_message;
           ASSERT_TRUE(  force_torque_sensor_->get_values_as_message(  temp_message ) );
           ASSERT_EQ(  temp_message.force.x,   force_values_[0] );
           ASSERT_EQ(  temp_message.force.y,   force_values_[1] );
           ASSERT_EQ(  temp_message.force.z,   force_values_[2] );
           ASSERT_EQ(  temp_message.torque.x,   torque_values_[0] );
           ASSERT_EQ(  temp_message.torque.y,   torque_values_[1] );
           ASSERT_EQ(  temp_message.torque.z,   torque_values_[2] );
          
           // release the state_interfaces_
           force_torque_sensor_->release_interfaces(   );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  force_torque_sensor_->state_interfaces_.size(   ),   0u );
          }

ros2_control/controller_interface/test/test_force_torque_sensor.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #ifndef TEST_FORCE_TORQUE_SENSOR_HPP_
          #define TEST_FORCE_TORQUE_SENSOR_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "semantic_components/force_torque_sensor.hpp"
          
          // implementing and friending so we can access member variables
      31  class TestableForceTorqueSensor : public semantic_components::ForceTorqueSensor
          {
      33   FRIEND_TEST(  ForceTorqueSensorTest,   validate_all_with_default_names );
      34   FRIEND_TEST(  ForceTorqueSensorTest,   validate_all_with_custom_names );
      35   FRIEND_TEST(  ForceTorqueSensorTest,   validate_all_custom_names );
          
          public:
           // Use generation of interface names
      39   explicit TestableForceTorqueSensor(  const std::string & name ) : ForceTorqueSensor(  name ) {}
           // Use custom interface names
      41   explicit TestableForceTorqueSensor(  
      42   const std::string & interface_force_x,   const std::string & interface_force_y,  
      43   const std::string & interface_force_z,   const std::string & interface_torque_x,  
      44   const std::string & interface_torque_y,   const std::string & interface_torque_z )
           : ForceTorqueSensor(  
           interface_force_x,   interface_force_y,   interface_force_z,   interface_torque_x,  
           interface_torque_y,   interface_torque_z )
           {
           }
          
           virtual ~TestableForceTorqueSensor(   ) = default;
          };
          
      54  class ForceTorqueSensorTest : public ::testing::Test
          {
          public:
      57   void SetUp(   )
           {
           full_interface_names_.reserve(  size_ );
           for (  auto index = 0u; index < size_; ++index )
           {
           full_interface_names_.emplace_back(  sensor_name_ + "/" + fts_interface_names_[index] );
           }
           }
          
      66   void TearDown(   );
          
          protected:
      69   const size_t size_ = 6;
      70   const std::string sensor_name_ = "test_FTS";
      71   std::array<double,   3> force_values_ = {1.1,   2.2,   3.3};
      72   std::array<double,   3> torque_values_ = {4.4,   5.5,   6.6};
      73   std::unique_ptr<TestableForceTorqueSensor> force_torque_sensor_;
          
      75   std::vector<std::string> full_interface_names_;
      76   const std::vector<std::string> fts_interface_names_ = {"force.x",   "force.y",   "force.z",  
           "torque.x",   "torque.y",   "torque.z"};
          };
          
          #endif // TEST_FORCE_TORQUE_SENSOR_HPP_

ros2_control/controller_interface/test/test_imu_sensor.cpp

       1  // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #include "test_imu_sensor.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
      25  void IMUSensorTest::TearDown(   ) { imu_sensor_.reset(  nullptr ); }
          
      27  TEST_F(  IMUSensorTest,   validate_all )
          {
           // create the IMU sensor
           imu_sensor_ = std::make_unique<TestableIMUSensor>(  sensor_name_ );
          
           // validate the component name
           ASSERT_EQ(  imu_sensor_->name_,   sensor_name_ );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  imu_sensor_->interface_names_.size(   ),   size_ );
           ASSERT_EQ(  imu_sensor_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the default interface_names_
           ASSERT_TRUE(  std::equal(  
           imu_sensor_->interface_names_.begin(   ),   imu_sensor_->interface_names_.end(   ),  
           full_interface_names_.begin(   ),   full_interface_names_.end(   ) ) );
          
           // get the interface names
           std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names(   );
          
           // assign values to orientation
           hardware_interface::StateInterface orientation_x{
           sensor_name_,   imu_interface_names_[0],   &orientation_values_[0]};
           hardware_interface::StateInterface orientation_y{
           sensor_name_,   imu_interface_names_[1],   &orientation_values_[1]};
           hardware_interface::StateInterface orientation_z{
           sensor_name_,   imu_interface_names_[2],   &orientation_values_[2]};
           hardware_interface::StateInterface orientation_w{
           sensor_name_,   imu_interface_names_[3],   &orientation_values_[4]};
          
           // assign values to angular velocity
           hardware_interface::StateInterface angular_velocity_x{
           sensor_name_,   imu_interface_names_[4],   &angular_velocity_values_[0]};
           hardware_interface::StateInterface angular_velocity_y{
           sensor_name_,   imu_interface_names_[5],   &angular_velocity_values_[1]};
           hardware_interface::StateInterface angular_velocity_z{
           sensor_name_,   imu_interface_names_[6],   &angular_velocity_values_[2]};
          
           // assign values to linear acceleration
           hardware_interface::StateInterface linear_acceleration_x{
           sensor_name_,   imu_interface_names_[7],   &linear_acceleration_values_[0]};
           hardware_interface::StateInterface linear_acceleration_y{
           sensor_name_,   imu_interface_names_[8],   &linear_acceleration_values_[1]};
           hardware_interface::StateInterface linear_acceleration_z{
           sensor_name_,   imu_interface_names_[9],   &linear_acceleration_values_[2]};
          
           // create local state interface vector
           std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
           temp_state_interfaces.reserve(  10 );
          
           // insert the interfaces in jumbled sequence
           temp_state_interfaces.emplace_back(  angular_velocity_y );
           temp_state_interfaces.emplace_back(  orientation_y );
           temp_state_interfaces.emplace_back(  linear_acceleration_y );
           temp_state_interfaces.emplace_back(  orientation_x );
           temp_state_interfaces.emplace_back(  linear_acceleration_z );
           temp_state_interfaces.emplace_back(  angular_velocity_z );
           temp_state_interfaces.emplace_back(  orientation_z );
           temp_state_interfaces.emplace_back(  orientation_w );
           temp_state_interfaces.emplace_back(  angular_velocity_x );
           temp_state_interfaces.emplace_back(  linear_acceleration_x );
          
           // now call the function to make them in order like interface_names
           imu_sensor_->assign_loaned_state_interfaces(  temp_state_interfaces );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  imu_sensor_->state_interfaces_.size(   ),   size_ );
          
           // validate the orientation values
           std::array<double,   4> temp_orientation_values = imu_sensor_->get_orientation(   );
           ASSERT_EQ(  temp_orientation_values,   orientation_values_ );
          
           // validate the angular_velocity values
           std::array<double,   3> temp_angular_velocity_values = imu_sensor_->get_angular_velocity(   );
           ASSERT_EQ(  temp_angular_velocity_values,   angular_velocity_values_ );
          
           // validate the linear_acceleration values
           std::array<double,   3> temp_linear_acceleration_values = imu_sensor_->get_linear_acceleration(   );
           ASSERT_EQ(  temp_linear_acceleration_values,   linear_acceleration_values_ );
          
           // validate get_values_as_message
           sensor_msgs::msg::Imu temp_message;
           ASSERT_TRUE(  imu_sensor_->get_values_as_message(  temp_message ) );
           ASSERT_EQ(  temp_message.orientation.x,   orientation_values_[0] );
           ASSERT_EQ(  temp_message.orientation.y,   orientation_values_[1] );
           ASSERT_EQ(  temp_message.orientation.z,   orientation_values_[2] );
           ASSERT_EQ(  temp_message.orientation.w,   orientation_values_[3] );
           ASSERT_EQ(  temp_message.angular_velocity.x,   angular_velocity_values_[0] );
           ASSERT_EQ(  temp_message.angular_velocity.y,   angular_velocity_values_[1] );
           ASSERT_EQ(  temp_message.angular_velocity.z,   angular_velocity_values_[2] );
           ASSERT_EQ(  temp_message.linear_acceleration.x,   linear_acceleration_values_[0] );
           ASSERT_EQ(  temp_message.linear_acceleration.y,   linear_acceleration_values_[1] );
           ASSERT_EQ(  temp_message.linear_acceleration.z,   linear_acceleration_values_[2] );
          
           // release the state_interfaces_
           imu_sensor_->release_interfaces(   );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  imu_sensor_->state_interfaces_.size(   ),   0u );
          }

ros2_control/controller_interface/test/test_imu_sensor.hpp

       1  // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #ifndef TEST_IMU_SENSOR_HPP_
          #define TEST_IMU_SENSOR_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "semantic_components/imu_sensor.hpp"
          
          // implementing and friending so we can access member variables
      31  class TestableIMUSensor : public semantic_components::IMUSensor
          {
      33   FRIEND_TEST(  IMUSensorTest,   validate_all );
          
          public:
           // Use generation of interface names
      37   explicit TestableIMUSensor(  const std::string & name ) : IMUSensor(  name ) {}
          
           virtual ~TestableIMUSensor(   ) = default;
          };
          
      42  class IMUSensorTest : public ::testing::Test
          {
          public:
      45   void SetUp(   )
           {
           full_interface_names_.reserve(  size_ );
           for (  auto index = 0u; index < size_; ++index )
           {
           full_interface_names_.emplace_back(  sensor_name_ + "/" + imu_interface_names_[index] );
           }
           }
          
      54   void TearDown(   );
          
          protected:
      57   const size_t size_ = 10;
      58   const std::string sensor_name_ = "test_IMU";
      59   std::array<double,   4> orientation_values_ = {1.1,   2.2,   3.3,   4.4};
      60   std::array<double,   3> angular_velocity_values_ = {4.4,   5.5,   6.6};
      61   std::array<double,   3> linear_acceleration_values_ = {4.4,   5.5,   6.6};
      62   std::unique_ptr<TestableIMUSensor> imu_sensor_;
          
      64   std::vector<std::string> full_interface_names_;
      65   const std::vector<std::string> imu_interface_names_ = {
           "orientation.x",   "orientation.y",   "orientation.z",   "orientation.w",  
           "angular_velocity.x",   "angular_velocity.y",   "angular_velocity.z",   "linear_acceleration.x",  
           "linear_acceleration.y",   "linear_acceleration.z"};
          };
          
          #endif // TEST_IMU_SENSOR_HPP_

ros2_control/controller_interface/test/test_semantic_component_interface.cpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #include "test_semantic_component_interface.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
      25  void SemanticComponentInterfaceTest::TearDown(   ) { semantic_component_.reset(  nullptr ); }
          
      27  TEST_F(  SemanticComponentInterfaceTest,   validate_default_names )
          {
           // create 'test_component' with 5 interfaces using default naming
           // e.g. test_component_1,   test_component_2 so on...
           semantic_component_ =
           std::make_unique<TestableSemanticComponentInterface>(  component_name_,   size_ );
          
           // validate the component name
           ASSERT_EQ(  semantic_component_->name_,   component_name_ );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  semantic_component_->interface_names_.capacity(   ),   size_ );
           ASSERT_EQ(  semantic_component_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the interface_names_
           std::vector<std::string> interface_names = semantic_component_->get_state_interface_names(   );
           ASSERT_EQ(  interface_names,   semantic_component_->interface_names_ );
          
           ASSERT_EQ(  interface_names.size(   ),   size_ );
           ASSERT_EQ(  interface_names[0],   component_name_ + "/1" );
           ASSERT_EQ(  interface_names[1],   component_name_ + "/2" );
           ASSERT_EQ(  interface_names[2],   component_name_ + "/3" );
           ASSERT_EQ(  interface_names[3],   component_name_ + "/4" );
           ASSERT_EQ(  interface_names[4],   component_name_ + "/5" );
          }
          
      54  TEST_F(  SemanticComponentInterfaceTest,   validate_custom_names )
          {
           // create a component with 5 interfaces using custom naming
           // as defined in the constructor
           semantic_component_ = std::make_unique<TestableSemanticComponentInterface>(  size_ );
          
           // validate the component name
           ASSERT_EQ(  semantic_component_->name_,   semantic_component_->test_name_ );
          
           // validate the space reserved for interface_names_ and state_interfaces_
           // Note : Using capacity(   ) for state_interfaces_ as no such interfaces are defined yet
           ASSERT_EQ(  semantic_component_->interface_names_.capacity(   ),   size_ );
           ASSERT_EQ(  semantic_component_->state_interfaces_.capacity(   ),   size_ );
          
           // validate the interface_names_
           std::vector<std::string> interface_names = semantic_component_->get_state_interface_names(   );
           ASSERT_TRUE(  std::equal(  
           semantic_component_->interface_names_.begin(   ),   semantic_component_->interface_names_.end(   ),  
           interface_names.begin(   ),   interface_names.end(   ) ) );
          
           ASSERT_EQ(  interface_names.size(   ),   size_ );
           ASSERT_EQ(  interface_names[0],   semantic_component_->test_name_ + "/i5" );
           ASSERT_EQ(  interface_names[1],   semantic_component_->test_name_ + "/i6" );
           ASSERT_EQ(  interface_names[2],   semantic_component_->test_name_ + "/i7" );
           ASSERT_EQ(  interface_names[3],   semantic_component_->test_name_ + "/i8" );
           ASSERT_EQ(  interface_names[4],   semantic_component_->test_name_ + "/i9" );
          }
          
      82  TEST_F(  SemanticComponentInterfaceTest,   validate_state_interfaces )
          {
           // create 'test_component' with 5 interfaces using default naming
           // e.g. test_component_1,   test_component_2 so on...
           semantic_component_ =
           std::make_unique<TestableSemanticComponentInterface>(  component_name_,   size_ );
          
           // generate the interface_names_
           std::vector<std::string> interface_names = semantic_component_->get_state_interface_names(   );
          
           // validate assign_loaned_state_interfaces
           // create interfaces and assign values to it
           std::vector<double> interface_values = {1.1,   3.3,   5.5};
          
           // assign 1.1 to interface_1,   3.3 to interface_3 and 5.5 to interface_5
           hardware_interface::StateInterface interface_1{component_name_,   "1",   &interface_values[0]};
           hardware_interface::StateInterface interface_3{component_name_,   "3",   &interface_values[1]};
           hardware_interface::StateInterface interface_5{component_name_,   "5",   &interface_values[2]};
          
           // create local state interface vector
           std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
          
           // insert the interfaces in jumbled sequence
           temp_state_interfaces.emplace_back(  interface_5 );
           temp_state_interfaces.emplace_back(  interface_1 );
           temp_state_interfaces.emplace_back(  interface_3 );
          
           // now call the function to make them in order like interface_names
           semantic_component_->assign_loaned_state_interfaces(  temp_state_interfaces );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  semantic_component_->state_interfaces_.size(   ),   3u );
          
           // validate the values of state_interfaces_ which should be
           // in order as per interface_names_
           std::vector<double> temp_values;
           ASSERT_FALSE(  semantic_component_->get_values(  temp_values ) );
          
           // reserve memory for the vector and call get_values
           temp_values.reserve(  3 );
           ASSERT_TRUE(  semantic_component_->get_values(  temp_values ) );
           ASSERT_EQ(  temp_values,   interface_values );
          
           // release the state_interfaces_
           semantic_component_->release_interfaces(   );
          
           // validate the count of state_interfaces_
           ASSERT_EQ(  semantic_component_->state_interfaces_.size(   ),   0u );
          
           // validate that release_interfaces(   ) does not touch interface_names_
           ASSERT_TRUE(  std::equal(  
           semantic_component_->interface_names_.begin(   ),   semantic_component_->interface_names_.end(   ),  
           interface_names.begin(   ),   interface_names.end(   ) ) );
          }

ros2_control/controller_interface/test/test_semantic_component_interface.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #ifndef TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
          #define TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          
          #include "geometry_msgs/msg/wrench.hpp"
          #include "gmock/gmock.h"
          #include "semantic_components/semantic_component_interface.hpp"
          
          // implementing and friending so we can access member variables
      30  class TestableSemanticComponentInterface
      31  : public semantic_components::SemanticComponentInterface<geometry_msgs::msg::Wrench>
          {
      33   FRIEND_TEST(  SemanticComponentInterfaceTest,   validate_default_names );
      34   FRIEND_TEST(  SemanticComponentInterfaceTest,   validate_custom_names );
      35   FRIEND_TEST(  SemanticComponentInterfaceTest,   validate_state_interfaces );
          
          public:
           // Use generation of interface names
      39   explicit TestableSemanticComponentInterface(  const std::string & name,   size_t size )
           : SemanticComponentInterface<geometry_msgs::msg::Wrench>(  name,   size )
           {
           }
           // Use custom interface names
      44   explicit TestableSemanticComponentInterface(  size_t size )
           : SemanticComponentInterface(  "TestSemanticComponent",   size )
           {
           // generate the interface_names_
           for (  auto i = 0u; i < size; ++i )
           {
           interface_names_.emplace_back(  
           std::string(  "TestSemanticComponent" ) + "/i" + std::to_string(  i + 5 ) );
           }
           }
          
           virtual ~TestableSemanticComponentInterface(   ) = default;
          
           std::string test_name_ = "TestSemanticComponent";
          };
          
      60  class SemanticComponentInterfaceTest : public ::testing::Test
          {
          public:
      63   void TearDown(   );
          
          protected:
      66   const std::string component_name_ = "test_component";
      67   const size_t size_ = 5;
      68   std::unique_ptr<TestableSemanticComponentInterface> semantic_component_;
          };
          
          #endif // TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_

ros2_control/controller_manager/include/controller_manager/controller_manager.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
          #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
          
          #include <map>
          #include <memory>
          #include <string>
          #include <tuple>
          #include <unordered_map>
          #include <vector>
          
          #include "controller_interface/chainable_controller_interface.hpp"
          #include "controller_interface/controller_interface.hpp"
          #include "controller_interface/controller_interface_base.hpp"
          
          #include "controller_manager/controller_spec.hpp"
          #include "controller_manager/visibility_control.h"
          #include "controller_manager_msgs/srv/configure_controller.hpp"
          #include "controller_manager_msgs/srv/list_controller_types.hpp"
          #include "controller_manager_msgs/srv/list_controllers.hpp"
          #include "controller_manager_msgs/srv/list_hardware_components.hpp"
          #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
          #include "controller_manager_msgs/srv/load_controller.hpp"
          #include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
          #include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
          #include "controller_manager_msgs/srv/switch_controller.hpp"
          #include "controller_manager_msgs/srv/unload_controller.hpp"
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/resource_manager.hpp"
          
          #include "pluginlib/class_loader.hpp"
          
          #include "rclcpp/executor.hpp"
          #include "rclcpp/node.hpp"
          #include "rclcpp/node_interfaces/node_logging_interface.hpp"
          #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
          #include "rclcpp/parameter.hpp"
          
          namespace controller_manager
          {
          using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
          
      57  class ControllerManager : public rclcpp::Node
          {
          public:
           static constexpr bool kWaitForAllResources = false;
           static constexpr auto kInfiniteTimeout = 0;
          
           CONTROLLER_MANAGER_PUBLIC
           ControllerManager(  
           std::unique_ptr<hardware_interface::ResourceManager> resource_manager,  
           std::shared_ptr<rclcpp::Executor> executor,  
           const std::string & manager_node_name = "controller_manager",  
           const std::string & namespace_ = "" );
          
           CONTROLLER_MANAGER_PUBLIC
           ControllerManager(  
           std::shared_ptr<rclcpp::Executor> executor,  
           const std::string & manager_node_name = "controller_manager",  
           const std::string & namespace_ = "" );
          
           CONTROLLER_MANAGER_PUBLIC
           virtual ~ControllerManager(   ) = default;
          
           CONTROLLER_MANAGER_PUBLIC
           void init_resource_manager(  const std::string & robot_description );
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::ControllerInterfaceBaseSharedPtr load_controller(  
           const std::string & controller_name,   const std::string & controller_type );
          
           /// load_controller loads a controller by name,   the type must be defined in the parameter server.
           /**
           * \param[in] controller_name as a string.
           * \return controller
           * \see Documentation in controller_manager_msgs/LoadController.srv
           */
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::ControllerInterfaceBaseSharedPtr load_controller(  
           const std::string & controller_name );
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type unload_controller(  const std::string & controller_name );
          
           CONTROLLER_MANAGER_PUBLIC
           std::vector<ControllerSpec> get_loaded_controllers(   ) const;
          
           template <
           typename T,   typename std::enable_if<
           std::is_convertible<T *,   controller_interface::ControllerInterfaceBase *>::value,  
           T>::type * = nullptr>
           controller_interface::ControllerInterfaceBaseSharedPtr add_controller(  
           std::shared_ptr<T> controller,   const std::string & controller_name,  
           const std::string & controller_type )
           {
           ControllerSpec controller_spec;
           controller_spec.c = controller;
           controller_spec.info.name = controller_name;
           controller_spec.info.type = controller_type;
           return add_controller_impl(  controller_spec );
           }
          
           /// configure_controller Configure controller by name calling their "configure" method.
           /**
           * \param[in] controller_name as a string.
           * \return configure controller response
           * \see Documentation in controller_manager_msgs/ConfigureController.srv
           */
           CONTROLLER_MANAGER_PUBLIC
     124   controller_interface::return_type configure_controller(  const std::string & controller_name );
          
           /// switch_controller Stops some controllers and start others.
           /**
           * \param[in] start_controllers is a list of controllers to start
           * \param[in] stop_controllers is a list of controllers to stop
           * \param[in] set level of strictness (  BEST_EFFORT or STRICT )
           * \see Documentation in controller_manager_msgs/SwitchController.srv
           */
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type switch_controller(  
           const std::vector<std::string> & start_controllers,  
           const std::vector<std::string> & stop_controllers,   int strictness,  
           bool activate_asap = kWaitForAllResources,  
           const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(  kInfiniteTimeout ) );
          
     140   CONTROLLER_MANAGER_PUBLIC
           void read(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period );
          
     147   CONTROLLER_MANAGER_PUBLIC
           void write(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           /// Deterministic (  real-time safe ) callback group,   e.g.,   update function.
           /**
           * Deterministic (  real-time safe ) callback group for the update function. Default behavior
           * is read hardware,   update controller and finally write new values to the hardware.
           */
           // TODO(  anyone ): Due to issues with the MutliThreadedExecutor,   this control loop does not rely on
           // the executor (  see issue #260 ).
           // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
          
           // Per controller update rate support
     160   CONTROLLER_MANAGER_PUBLIC
           unsigned int get_update_rate(   ) const;
          
          protected:
     164   CONTROLLER_MANAGER_PUBLIC
           void init_services(   );
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(  
           const ControllerSpec & controller );
          
     171   CONTROLLER_MANAGER_PUBLIC
           void manage_switch(   );
          
     174   CONTROLLER_MANAGER_PUBLIC
           void deactivate_controllers(   );
          
           /**
           * Switch chained mode for all the controllers with respect to the following cases:
           * - a preceding controller is getting activated --> switch controller to chained mode;
           * - all preceding controllers are deactivated --> switch controller from chained mode.
           *
           * \param[in] chained_mode_switch_list list of controller to switch chained mode.
           * \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode.
           */
           CONTROLLER_MANAGER_PUBLIC
     186   void switch_chained_mode(  
           const std::vector<std::string> & chained_mode_switch_list,   bool to_chained_mode );
          
     189   CONTROLLER_MANAGER_PUBLIC
           void activate_controllers(   );
          
     192   CONTROLLER_MANAGER_PUBLIC
           void activate_controllers_asap(   );
          
           CONTROLLER_MANAGER_PUBLIC
     196   void list_controllers_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response );
          
     200   CONTROLLER_MANAGER_PUBLIC
           void list_hardware_interfaces_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response );
          
     205   CONTROLLER_MANAGER_PUBLIC
           void load_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response );
          
           CONTROLLER_MANAGER_PUBLIC
     211   void configure_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Response> response );
          
     215   CONTROLLER_MANAGER_PUBLIC
           void reload_controller_libraries_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response );
          
     220   CONTROLLER_MANAGER_PUBLIC
           void switch_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response );
          
           CONTROLLER_MANAGER_PUBLIC
     226   void unload_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response );
          
     230   CONTROLLER_MANAGER_PUBLIC
           void list_controller_types_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response );
          
     235   CONTROLLER_MANAGER_PUBLIC
           void list_hardware_components_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response );
          
           CONTROLLER_MANAGER_PUBLIC
     241   void set_hardware_component_state_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response );
          
           // Per controller update rate support
           unsigned int update_loop_counter_ = 0;
           unsigned int update_rate_ = 100;
     248   std::vector<std::vector<std::string>> chained_controllers_configuration_;
          
           std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
          
          private:
           std::vector<std::string> get_controller_names(   );
          
           /**
           * Clear request lists used when switching controllers. The lists are shared between "callback" and
           * "control loop" threads.
           */
           void clear_requests(   );
          
           /**
           * If a controller is deactivated all following controllers (  if any exist ) should be switched
           * 'from' the chained mode.
           *
           * \param[in] controllers list with controllers.
           */
           void propagate_deactivation_of_chained_mode(  const std::vector<ControllerSpec> & controllers );
          
           /// Check if all the following controllers will be in active state and in the chained mode
           /// after controllers' switch.
           /**
           * Check recursively that all following controllers of the @controller_it
           * - are already active,  
           * - will not be deactivated,  
           * - or will be activated.
           * The following controllers are added to the request to switch in the chained mode or removed
           * from the request to switch from the chained mode.
           *
           * For each controller the whole chain of following controllers is checked.
           *
           * NOTE: The automatically adding of following controller into starting list is not implemented
           * yet.
           *
           * \param[in] controllers list with controllers.
           * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
           * controllers will be automatically added to the activate request list if they are not in the
           * deactivate request.
           * \param[in] controller_it iterator to the controller for which the following controllers are
           * checked.
           *
           * \returns return_type::OK if all following controllers pass the checks,   otherwise
           * return_type::ERROR.
           */
           controller_interface::return_type check_following_controllers_for_activate(  
           const std::vector<ControllerSpec> & controllers,   int strictness,  
           const ControllersListIterator controller_it );
          
           /// Check if all the preceding controllers will be in inactive state after controllers' switch.
           /**
           * Check that all preceding controllers of the @controller_it
           * - are inactive,  
           * - will be deactivated,  
           * - and will not be activated.
           *
           * NOTE: The automatically adding of preceding controllers into stopping list is not implemented
           * yet.
           *
           * \param[in] controllers list with controllers.
           * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding
           * controllers will be automatically added to the deactivate request list.
           * \param[in] controller_it iterator to the controller for which the preceding controllers are
           * checked.
           *
           * \returns return_type::OK if all preceding controllers pass the checks,   otherwise
           * return_type::ERROR.
           */
           controller_interface::return_type check_preceeding_controllers_for_deactivate(  
           const std::vector<ControllerSpec> & controllers,   int strictness,  
           const ControllersListIterator controller_it );
          
           std::shared_ptr<rclcpp::Executor> executor_;
          
           std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
           std::shared_ptr<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>
           chainable_loader_;
          
           /// Best effort (  non real-time safe ) callback group,   e.g.,   service callbacks.
           /**
           * Best effort (  non real-time safe ) callback group for callbacks that can possibly break
           * real-time requirements,   for example,   service callbacks.
           */
           rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
          
           /**
           * The RTControllerListWrapper class wraps a double-buffered list of controllers
           * to avoid needing to lock the real-time thread when switching controllers in
           * the non-real-time thread.
           *
           * There's always an "updated" list and an "outdated" one
           * There's always an "used by rt" list and an "unused by rt" list
           *
           * The updated state changes on the switch_updated_list(   )
           * The rt usage state changes on the update_and_get_used_by_rt_list(   )
           */
           class RTControllerListWrapper
           {
           // *INDENT-OFF*
           public:
           // *INDENT-ON*
           /// update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list
           /**
           * \warning Should only be called by the RT thread,   no one should modify the
           * updated list while it's being used
           * \return reference to the updated list
           */
           std::vector<ControllerSpec> & update_and_get_used_by_rt_list(   );
          
           /**
           * get_unused_list Waits until the "outdated" and "unused by rt"
           * lists match and returns a reference to it
           * This referenced list can be modified safely until switch_updated_controller_list(   )
           * is called,   at this point the RT thread may start using it at any time
           * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
           */
           std::vector<ControllerSpec> & get_unused_list(  
           const std::lock_guard<std::recursive_mutex> & guard );
          
           /// get_updated_list Returns a const reference to the most updated list.
           /**
           * \warning May or may not being used by the realtime thread,   read-only reference for safety
           * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
           */
           const std::vector<ControllerSpec> & get_updated_list(  
           const std::lock_guard<std::recursive_mutex> & guard ) const;
          
           /**
           * switch_updated_list Switches the "updated" and "outdated" lists,   and waits
           * until the RT thread is using the new "updated" list.
           * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
           */
           void switch_updated_list(  const std::lock_guard<std::recursive_mutex> & guard );
          
           // Mutex protecting the controllers list
           // must be acquired before using any list other than the "used by rt"
           mutable std::recursive_mutex controllers_lock_;
          
           // *INDENT-OFF*
           private:
           // *INDENT-ON*
           /// get_other_list get the list not pointed by index
           /**
           * \param[in] index int
           */
           int get_other_list(  int index ) const;
          
           void wait_until_rt_not_using(  
           int index,   std::chrono::microseconds sleep_delay = std::chrono::microseconds(  200 ) ) const;
          
           std::vector<ControllerSpec> controllers_lists_[2];
           /// The index of the controller list with the most updated information
           int updated_controllers_index_ = 0;
           /// The index of the controllers list being used in the real-time thread.
           int used_by_realtime_controllers_index_ = -1;
           };
          
           RTControllerListWrapper rt_controllers_wrapper_;
           /// mutex copied from ROS1 Control,   protects service callbacks
           /// not needed if we're guaranteed that the callbacks don't come from multiple threads
           std::mutex services_lock_;
           rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
           list_controllers_service_;
           rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
           list_controller_types_service_;
           rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr load_controller_service_;
           rclcpp::Service<controller_manager_msgs::srv::ConfigureController>::SharedPtr
           configure_controller_service_;
           rclcpp::Service<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr
           reload_controller_libraries_service_;
           rclcpp::Service<controller_manager_msgs::srv::SwitchController>::SharedPtr
           switch_controller_service_;
           rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
           unload_controller_service_;
          
           rclcpp::Service<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr
           list_hardware_components_service_;
           rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
           list_hardware_interfaces_service_;
           rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
           set_hardware_component_state_service_;
          
           std::vector<std::string> activate_request_,   deactivate_request_;
           std::vector<std::string> to_chained_mode_request_,   from_chained_mode_request_;
           std::vector<std::string> activate_command_interface_request_,  
           deactivate_command_interface_request_;
          
           struct SwitchParams
           {
           bool do_switch = {false};
           bool started = {false};
           rclcpp::Time init_time = {rclcpp::Time::max(   )};
          
           // Switch options
           int strictness = {0};
           bool activate_asap = {false};
           rclcpp::Duration timeout = rclcpp::Duration{0,   0};
           };
          
           SwitchParams switch_params_;
          };
          
          } // namespace controller_manager
          
          #endif // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_

ros2_control/controller_manager/include/controller_manager/controller_spec.hpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Wim Meeussen
           */
          
          #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
          #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
          
          #include <map>
          #include <string>
          #include <vector>
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/controller_info.hpp"
          
          namespace controller_manager
          {
          /// Controller Specification
          /**
           * This struct contains both a pointer to a given controller,   \ref c,   as well
           * as information about the controller,   \ref info.
           *
           */
          struct ControllerSpec
          {
           hardware_interface::ControllerInfo info;
           controller_interface::ControllerInterfaceBaseSharedPtr c;
          };
          
          } // namespace controller_manager
          #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_

ros2_control/controller_manager/include/controller_manager/realtime.hpp

       1  // Copyright 2022 PickNik Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_MANAGER__REALTIME_HPP_
          #define CONTROLLER_MANAGER__REALTIME_HPP_
          
          namespace controller_manager
          {
          /**
           * Detect if realtime kernel is present.
           * \returns true if realtime kernel is detected
           */
      24  bool has_realtime_kernel(   );
          
          /**
           * Configure SCHED_FIFO thread priority for the thread that calls this function
           * \param[in] priority the priority of this thread from 0-99
           * \returns true if configuring scheduler succeeded
           */
      31  bool configure_sched_fifo(  int priority );
          
          } // namespace controller_manager
          
          #endif // CONTROLLER_MANAGER__REALTIME_HPP_

ros2_control/controller_manager/include/controller_manager/visibility_control.h

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
          #define CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define CONTROLLER_MANAGER_EXPORT __attribute__(  (  dllexport ) )
          #define CONTROLLER_MANAGER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define CONTROLLER_MANAGER_EXPORT __declspec(  dllexport )
          #define CONTROLLER_MANAGER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef CONTROLLER_MANAGER_BUILDING_DLL
          #define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_EXPORT
          #else
          #define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_IMPORT
          #endif
          #define CONTROLLER_MANAGER_PUBLIC_TYPE CONTROLLER_MANAGER_PUBLIC
          #define CONTROLLER_MANAGER_LOCAL
          #else
          #define CONTROLLER_MANAGER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define CONTROLLER_MANAGER_IMPORT
          #if __GNUC__ >= 4
          #define CONTROLLER_MANAGER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define CONTROLLER_MANAGER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define CONTROLLER_MANAGER_PUBLIC
          #define CONTROLLER_MANAGER_LOCAL
          #endif
          #define CONTROLLER_MANAGER_PUBLIC_TYPE
          #endif
          
          #endif // CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_

ros2_control/controller_manager/src/controller_manager.cpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "controller_manager/controller_manager.hpp"
          
          #include <list>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "controller_interface/controller_interface_base.hpp"
          #include "controller_manager_msgs/msg/hardware_component_state.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace // utility
          {
      32  static constexpr const char * kControllerInterfaceNamespace = "controller_interface";
      33  static constexpr const char * kControllerInterfaceClassName =
           "controller_interface::ControllerInterface";
      35  static constexpr const char * kChainableControllerInterfaceClassName =
           "controller_interface::ChainableControllerInterface";
          
          // Changed services history QoS to keep all so we don't lose any client service calls
          static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = {
           RMW_QOS_POLICY_HISTORY_KEEP_ALL,  
           1,   // message queue depth
           RMW_QOS_POLICY_RELIABILITY_RELIABLE,  
           RMW_QOS_POLICY_DURABILITY_VOLATILE,  
           RMW_QOS_DEADLINE_DEFAULT,  
           RMW_QOS_LIFESPAN_DEFAULT,  
           RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,  
           RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,  
           false};
          
      50  inline bool is_controller_inactive(  const controller_interface::ControllerInterfaceBase & controller )
          {
           return controller.get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
          }
          
      55  inline bool is_controller_inactive(  
           const controller_interface::ControllerInterfaceBaseSharedPtr & controller )
          {
           return is_controller_inactive(  *controller );
          }
          
      61  inline bool is_controller_active(  const controller_interface::ControllerInterfaceBase & controller )
          {
           return controller.get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
          }
          
      66  inline bool is_controller_active(  
           const controller_interface::ControllerInterfaceBaseSharedPtr & controller )
          {
           return is_controller_active(  *controller );
          }
          
      72  bool controller_name_compare(  const controller_manager::ControllerSpec & a,   const std::string & name )
          {
           return a.info.name == name;
          }
          
          /// Checks if a command interface belongs to a controller based on its prefix.
          /**
           * A command interface can be provided by a controller in which case is called "reference"
           * interface.
           * This means that the @interface_name starts with the name of a controller.
           *
           * \param[in] interface_name to be found in the map.
           * \param[in] controllers list of controllers to compare their names to interface's prefix.
           * \param[out] following_controller_it iterator to the following controller that reference interface
           * @interface_name belongs to.
           * \return true if interface has a controller name as prefix,   false otherwise.
           */
      89  bool command_interface_is_reference_interface_of_controller(  
           const std::string interface_name,  
           const std::vector<controller_manager::ControllerSpec> & controllers,  
           controller_manager::ControllersListIterator & following_controller_it )
          {
           auto split_pos = interface_name.find_first_of(  '/' );
           if (  split_pos == std::string::npos ) // '/' exist in the string (  should be always false )
           {
           RCLCPP_FATAL(  
           rclcpp::get_logger(  "ControllerManager::utils" ),  
           "Character '/',   was not find in the interface name '%s'. This should never happen. "
           "Stop the controller manager immediately and restart it.",  
           interface_name.c_str(   ) );
           throw std::runtime_error(  "Mismatched interface name. See the FATAL message above." );
           }
          
           auto interface_prefix = interface_name.substr(  0,   split_pos );
           following_controller_it = std::find_if(  
           controllers.begin(   ),   controllers.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   interface_prefix ) );
          
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "ControllerManager::utils" ),  
           "Deduced interface prefix '%s' - searching for the controller with the same name.",  
           interface_prefix.c_str(   ) );
          
           if (  following_controller_it == controllers.end(   ) )
           {
           RCLCPP_DEBUG(  
           rclcpp::get_logger(  "ControllerManager::utils" ),  
           "Required command interface '%s' with prefix '%s' is not reference interface.",  
           interface_name.c_str(   ),   interface_prefix.c_str(   ) );
          
           return false;
           }
           return true;
          }
          
          } // namespace
          
     129  namespace controller_manager
          {
          rclcpp::NodeOptions get_cm_node_options(   )
          {
           rclcpp::NodeOptions node_options;
           // Required for getting types of controllers to be loaded via service call
           node_options.allow_undeclared_parameters(  true );
           node_options.automatically_declare_parameters_from_overrides(  true );
           return node_options;
          }
          
          ControllerManager::ControllerManager(  
           std::shared_ptr<rclcpp::Executor> executor,   const std::string & manager_node_name,  
           const std::string & namespace_ )
          : rclcpp::Node(  manager_node_name,   namespace_,   get_cm_node_options(   ) ),  
           resource_manager_(  std::make_unique<hardware_interface::ResourceManager>(   ) ),  
           executor_(  executor ),  
           loader_(  std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(  
           kControllerInterfaceNamespace,   kControllerInterfaceClassName ) ),  
           chainable_loader_(  
           std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(  
           kControllerInterfaceNamespace,   kChainableControllerInterfaceClassName ) )
          {
           if (  !get_parameter(  "update_rate",   update_rate_ ) )
           {
           RCLCPP_WARN(  get_logger(   ),   "'update_rate' parameter not set,   using default value." );
           }
          
           std::string robot_description = "";
           get_parameter(  "robot_description",   robot_description );
           if (  robot_description.empty(   ) )
           {
           throw std::runtime_error(  "Unable to initialize resource manager,   no robot description found." );
           }
          
           init_resource_manager(  robot_description );
          
           init_services(   );
          }
          
          ControllerManager::ControllerManager(  
           std::unique_ptr<hardware_interface::ResourceManager> resource_manager,  
           std::shared_ptr<rclcpp::Executor> executor,   const std::string & manager_node_name,  
           const std::string & namespace_ )
          : rclcpp::Node(  manager_node_name,   namespace_,   get_cm_node_options(   ) ),  
           resource_manager_(  std::move(  resource_manager ) ),  
           executor_(  executor ),  
           loader_(  std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(  
           kControllerInterfaceNamespace,   kControllerInterfaceClassName ) ),  
           chainable_loader_(  
           std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(  
           kControllerInterfaceNamespace,   kChainableControllerInterfaceClassName ) )
          {
           init_services(   );
          }
          
          void ControllerManager::init_resource_manager(  const std::string & robot_description )
          {
           // TODO(  destogl ): manage this when there is an error - CM should not die because URDF is wrong...
           resource_manager_->load_urdf(  robot_description );
          
           using lifecycle_msgs::msg::State;
          
           std::vector<std::string> configure_components_on_start = std::vector<std::string>(  {} );
           get_parameter(  "configure_components_on_start",   configure_components_on_start );
           rclcpp_lifecycle::State inactive_state(  
           State::PRIMARY_STATE_INACTIVE,   hardware_interface::lifecycle_state_names::INACTIVE );
           for (  const auto & component : configure_components_on_start )
           {
           resource_manager_->set_component_state(  component,   inactive_state );
           }
          
           std::vector<std::string> activate_components_on_start = std::vector<std::string>(  {} );
           get_parameter(  "activate_components_on_start",   activate_components_on_start );
           rclcpp_lifecycle::State active_state(  
           State::PRIMARY_STATE_ACTIVE,   hardware_interface::lifecycle_state_names::ACTIVE );
           for (  const auto & component : activate_components_on_start )
           {
           resource_manager_->set_component_state(  component,   active_state );
           }
          
           // if both parameter are empty or non-existing preserve behavior where all components are
           // activated per default
           if (  configure_components_on_start.empty(   ) && activate_components_on_start.empty(   ) )
           {
           resource_manager_->activate_all_components(   );
           }
          }
          
          void ControllerManager::init_services(   )
          {
           // TODO(  anyone ): Due to issues with the MutliThreadedExecutor,   this control loop does not rely on
           // the executor (  see issue #260 ).
           // deterministic_callback_group_ = create_callback_group(  
           // rclcpp::CallbackGroupType::MutuallyExclusive );
           best_effort_callback_group_ = create_callback_group(  rclcpp::CallbackGroupType::MutuallyExclusive );
          
           using namespace std::placeholders;
           list_controllers_service_ = create_service<controller_manager_msgs::srv::ListControllers>(  
           "~/list_controllers",   std::bind(  &ControllerManager::list_controllers_srv_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           list_controller_types_service_ =
           create_service<controller_manager_msgs::srv::ListControllerTypes>(  
           "~/list_controller_types",  
           std::bind(  &ControllerManager::list_controller_types_srv_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           load_controller_service_ = create_service<controller_manager_msgs::srv::LoadController>(  
           "~/load_controller",   std::bind(  &ControllerManager::load_controller_service_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           configure_controller_service_ = create_service<controller_manager_msgs::srv::ConfigureController>(  
           "~/configure_controller",  
           std::bind(  &ControllerManager::configure_controller_service_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           reload_controller_libraries_service_ =
           create_service<controller_manager_msgs::srv::ReloadControllerLibraries>(  
           "~/reload_controller_libraries",  
           std::bind(  &ControllerManager::reload_controller_libraries_service_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           switch_controller_service_ = create_service<controller_manager_msgs::srv::SwitchController>(  
           "~/switch_controller",  
           std::bind(  &ControllerManager::switch_controller_service_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           unload_controller_service_ = create_service<controller_manager_msgs::srv::UnloadController>(  
           "~/unload_controller",  
           std::bind(  &ControllerManager::unload_controller_service_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           list_hardware_components_service_ =
           create_service<controller_manager_msgs::srv::ListHardwareComponents>(  
           "~/list_hardware_components",  
           std::bind(  &ControllerManager::list_hardware_components_srv_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           list_hardware_interfaces_service_ =
           create_service<controller_manager_msgs::srv::ListHardwareInterfaces>(  
           "~/list_hardware_interfaces",  
           std::bind(  &ControllerManager::list_hardware_interfaces_srv_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
           set_hardware_component_state_service_ =
           create_service<controller_manager_msgs::srv::SetHardwareComponentState>(  
           "~/set_hardware_component_state",  
           std::bind(  &ControllerManager::set_hardware_component_state_srv_cb,   this,   _1,   _2 ),  
           rmw_qos_profile_services_hist_keep_all,   best_effort_callback_group_ );
          }
          
          controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller(  
           const std::string & controller_name,   const std::string & controller_type )
          {
           RCLCPP_INFO(  get_logger(   ),   "Loading controller '%s'",   controller_name.c_str(   ) );
          
           if (  
           !loader_->isClassAvailable(  controller_type ) &&
           !chainable_loader_->isClassAvailable(  controller_type ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "Loader for controller '%s' (  type '%s' ) not found.",   controller_name.c_str(   ),  
           controller_type.c_str(   ) );
           RCLCPP_INFO(  get_logger(   ),   "Available classes:" );
           for (  const auto & available_class : loader_->getDeclaredClasses(   ) )
           {
           RCLCPP_INFO(  get_logger(   ),   " %s",   available_class.c_str(   ) );
           }
           for (  const auto & available_class : chainable_loader_->getDeclaredClasses(   ) )
           {
           RCLCPP_INFO(  get_logger(   ),   " %s",   available_class.c_str(   ) );
           }
           return nullptr;
           }
           RCLCPP_DEBUG(  get_logger(   ),   "Loader for controller '%s' found.",   controller_name.c_str(   ) );
          
           controller_interface::ControllerInterfaceBaseSharedPtr controller;
          
           if (  loader_->isClassAvailable(  controller_type ) )
           {
           controller = loader_->createSharedInstance(  controller_type );
           }
           if (  chainable_loader_->isClassAvailable(  controller_type ) )
           {
           controller = chainable_loader_->createSharedInstance(  controller_type );
           }
          
           ControllerSpec controller_spec;
           controller_spec.c = controller;
           controller_spec.info.name = controller_name;
           controller_spec.info.type = controller_type;
          
           return add_controller_impl(  controller_spec );
          }
          
          controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller(  
           const std::string & controller_name )
          {
           const std::string param_name = controller_name + ".type";
           std::string controller_type;
          
           // We cannot declare the parameters for the controllers that will be loaded in the future,  
           // because they are plugins and we cannot be aware of all of them.
           // So when we're told to load a controller by name,   we need to declare the parameter if
           // we haven't done so,   and then read it.
          
           // Check if parameter has been declared
           if (  !has_parameter(  param_name ) )
           {
           declare_parameter(  param_name,   rclcpp::ParameterType::PARAMETER_STRING );
           }
           if (  !get_parameter(  param_name,   controller_type ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "The 'type' param was not defined for '%s'.",   controller_name.c_str(   ) );
           return nullptr;
           }
           return load_controller(  controller_name,   controller_type );
          }
          
          controller_interface::return_type ControllerManager::unload_controller(  
           const std::string & controller_name )
          {
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
           std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(  guard );
           const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(  guard );
          
           // Transfers the active controllers over,   skipping the one to be removed and the active ones.
           to = from;
          
           auto found_it = std::find_if(  
           to.begin(   ),   to.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   controller_name ) );
           if (  found_it == to.end(   ) )
           {
           // Fails if we could not remove the controllers
           to.clear(   );
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Could not unload controller with name '%s' because no controller with this name exists",  
           controller_name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           auto & controller = *found_it;
          
           if (  is_controller_active(  *controller.c ) )
           {
           to.clear(   );
           RCLCPP_ERROR(  
           get_logger(   ),   "Could not unload controller with name '%s' because it is still active",  
           controller_name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "Cleanup controller" );
           // TODO(  destogl ): remove reference interface if chainable; i.e.,   add a separate method for
           // cleaning-up controllers?
           controller.c->get_node(   )->cleanup(   );
           executor_->remove_node(  controller.c->get_node(   )->get_node_base_interface(   ) );
           to.erase(  found_it );
          
           // Destroys the old controllers list when the realtime thread is finished with it.
           RCLCPP_DEBUG(  get_logger(   ),   "Realtime switches over to new controller list" );
           rt_controllers_wrapper_.switch_updated_list(  guard );
           std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(  guard );
           RCLCPP_DEBUG(  get_logger(   ),   "Destruct controller" );
           new_unused_list.clear(   );
           RCLCPP_DEBUG(  get_logger(   ),   "Destruct controller finished" );
          
           RCLCPP_DEBUG(  get_logger(   ),   "Successfully unloaded controller '%s'",   controller_name.c_str(   ) );
           return controller_interface::return_type::OK;
          }
          
          std::vector<ControllerSpec> ControllerManager::get_loaded_controllers(   ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
           return rt_controllers_wrapper_.get_updated_list(  guard );
          }
          
          controller_interface::return_type ControllerManager::configure_controller(  
           const std::string & controller_name )
          {
           RCLCPP_INFO(  get_logger(   ),   "Configuring controller '%s'",   controller_name.c_str(   ) );
          
           const auto & controllers = get_loaded_controllers(   );
          
           auto found_it = std::find_if(  
           controllers.begin(   ),   controllers.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   controller_name ) );
          
           if (  found_it == controllers.end(   ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Could not configure controller with name '%s' because no controller with this name exists",  
           controller_name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           auto controller = found_it->c;
          
           auto state = controller->get_state(   );
           if (  
           state.id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
           state.id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "Controller '%s' can not be configured from '%s' state.",  
           controller_name.c_str(   ),   state.label(   ).c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           auto new_state = controller->get_state(   );
           if (  state.id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           RCLCPP_DEBUG(  
           get_logger(   ),   "Controller '%s' is cleaned-up before configuring",   controller_name.c_str(   ) );
           // TODO(  destogl ): remove reference interface if chainable; i.e.,   add a separate method for
           // cleaning-up controllers?
           new_state = controller->get_node(   )->cleanup(   );
           if (  new_state.id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "Controller '%s' can not be cleaned-up before configuring",  
           controller_name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           }
          
           new_state = controller->configure(   );
           if (  new_state.id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "After configuring,   controller '%s' is in state '%s' ,   expected inactive.",  
           controller_name.c_str(   ),   new_state.label(   ).c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
           if (  controller->is_chainable(   ) )
           {
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Controller '%s' is chainable. Interfaces are being exported to resource manager.",  
           controller_name.c_str(   ) );
           auto interfaces = controller->export_reference_interfaces(   );
           if (  interfaces.empty(   ) )
           {
           // TODO(  destogl ): Add test for this!
           RCLCPP_ERROR(  
           get_logger(   ),   "Controller '%s' is chainable,   but does not export any reference interfaces.",  
           controller_name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           resource_manager_->import_controller_reference_interfaces(  controller_name,   interfaces );
          
           // TODO(  destogl ): check and resort controllers in the vector
           }
          
           return controller_interface::return_type::OK;
          }
          
          void ControllerManager::clear_requests(   )
          {
           deactivate_request_.clear(   );
           activate_request_.clear(   );
           to_chained_mode_request_.clear(   );
           from_chained_mode_request_.clear(   );
           activate_command_interface_request_.clear(   );
           deactivate_command_interface_request_.clear(   );
          }
          
          controller_interface::return_type ControllerManager::switch_controller(  
           const std::vector<std::string> & activate_controllers,  
           const std::vector<std::string> & deactivate_controllers,   int strictness,   bool activate_asap,  
           const rclcpp::Duration & timeout )
          {
           switch_params_ = SwitchParams(   );
          
           if (  !deactivate_request_.empty(   ) || !activate_request_.empty(   ) )
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "The internal deactivate and activate request lists are not empty at the beginning of the "
           "switch_controller(   ) call. This should never happen." );
           throw std::runtime_error(  "CM's internal state is not correct. See the FATAL message above." );
           }
           if (  
           !deactivate_command_interface_request_.empty(   ) || !activate_command_interface_request_.empty(   ) )
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "The internal deactivate and activat requests command interface lists are not empty at the "
           "switch_controller(   ) call. This should never happen." );
           throw std::runtime_error(  "CM's internal state is not correct. See the FATAL message above." );
           }
           if (  !from_chained_mode_request_.empty(   ) || !to_chained_mode_request_.empty(   ) )
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "The internal 'from' and 'to' chained mode requests are not empty at the "
           "switch_controller(   ) call. This should never happen." );
           throw std::runtime_error(  "CM's internal state is not correct. See the FATAL message above." );
           }
           if (  strictness == 0 )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Controller Manager: to switch controllers you need to specify a "
           "strictness level of controller_manager_msgs::SwitchController::STRICT "
           "(  %d ) or ::BEST_EFFORT (  %d ). Defaulting to ::BEST_EFFORT",  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,  
           controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT );
           strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "Switching controllers:" );
           for (  const auto & controller : activate_controllers )
           {
           RCLCPP_DEBUG(  get_logger(   ),   "- Activating controller '%s'",   controller.c_str(   ) );
           }
           for (  const auto & controller : deactivate_controllers )
           {
           RCLCPP_DEBUG(  get_logger(   ),   "- Deactivating controller '%s'",   controller.c_str(   ) );
           }
          
           const auto list_controllers = [this,   strictness](  
           const std::vector<std::string> & controller_list,  
           std::vector<std::string> & request_list,  
           const std::string & action )
           {
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
          
           // list all controllers to (  de )activate
           for (  const auto & controller : controller_list )
           {
           const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list(  guard );
          
           auto found_it = std::find_if(  
           updated_controllers.begin(   ),   updated_controllers.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   controller ) );
          
           if (  found_it == updated_controllers.end(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Could not '%s' controller with name '%s' because no controller with this name exists",  
           action.c_str(   ),   controller.c_str(   ) );
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Aborting,   no controller is switched! (  'STRICT' switch )" );
           return controller_interface::return_type::ERROR;
           }
           }
           else
           {
           RCLCPP_DEBUG(  
           get_logger(   ),   "Found controller '%s' that needs to be %sed in list of controllers",  
           controller.c_str(   ),   action.c_str(   ) );
           request_list.push_back(  controller );
           }
           }
           RCLCPP_DEBUG(  
           get_logger(   ),   "'%s' request vector has size %i",   action.c_str(   ),   (  int )request_list.size(   ) );
          
           return controller_interface::return_type::OK;
           };
          
           // list all controllers to deactivate (  check if all controllers exist )
           auto ret = list_controllers(  deactivate_controllers,   deactivate_request_,   "deactivate" );
           if (  ret != controller_interface::return_type::OK )
           {
           deactivate_request_.clear(   );
           return ret;
           }
          
           // list all controllers to activate (  check if all controllers exist )
           ret = list_controllers(  activate_controllers,   activate_request_,   "activate" );
           if (  ret != controller_interface::return_type::OK )
           {
           deactivate_request_.clear(   );
           activate_request_.clear(   );
           return ret;
           }
          
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
          
           const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(  guard );
          
           // if a preceding controller is deactivated,   all first-level controllers should be switched 'from'
           // chained mode
           propagate_deactivation_of_chained_mode(  controllers );
          
           // check if controllers should be switched 'to' chained mode when controllers are activated
           for (  auto ctrl_it = activate_request_.begin(   ); ctrl_it != activate_request_.end(   ); ++ctrl_it )
           {
           auto controller_it = std::find_if(  
           controllers.begin(   ),   controllers.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   *ctrl_it ) );
           controller_interface::return_type ret = controller_interface::return_type::OK;
          
           // if controller is not inactive then do not do any following-controllers checks
           if (  !is_controller_inactive(  controller_it->c ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Controller with name '%s' is not inactive so its following"
           "controllers do not have to be checked,   because it cannot be activated.",  
           controller_it->info.name.c_str(   ) );
           ret = controller_interface::return_type::ERROR;
           }
           else
           {
           ret = check_following_controllers_for_activate(  controllers,   strictness,   controller_it );
           }
          
           if (  ret != controller_interface::return_type::OK )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Could not activate controller with name '%s'. Check above warnings for more details. "
           "Check the state of the controllers and their required interfaces using "
           "`ros2 control list_controllers -v` CLI to get more information.",  
           (  *ctrl_it ).c_str(   ) );
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT )
           {
           // TODO(  destogl ): automatic manipulation of the chain:
           // || strictness ==
           // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN );
           // remove controller that can not be activated from the activation request and step-back
           // iterator to correctly step to the next element in the list in the loop
           activate_request_.erase(  ctrl_it );
           --ctrl_it;
           }
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Aborting,   no controller is switched! (  ::STRICT switch )" );
           // reset all lists
           clear_requests(   );
           return controller_interface::return_type::ERROR;
           }
           }
           }
          
           // check if controllers should be deactivated if used in chained mode
           for (  auto ctrl_it = deactivate_request_.begin(   ); ctrl_it != deactivate_request_.end(   ); ++ctrl_it )
           {
           auto controller_it = std::find_if(  
           controllers.begin(   ),   controllers.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   *ctrl_it ) );
           controller_interface::return_type ret = controller_interface::return_type::OK;
          
           // if controller is not active then skip preceding-controllers checks
           if (  !is_controller_active(  controller_it->c ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),   "Controller with name '%s' can not be deactivated since it is not active.",  
           controller_it->info.name.c_str(   ) );
           ret = controller_interface::return_type::ERROR;
           }
           else
           {
           ret = check_preceeding_controllers_for_deactivate(  controllers,   strictness,   controller_it );
           }
          
           if (  ret != controller_interface::return_type::OK )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Could not deactivate controller with name '%s'. Check above warnings for more details. "
           "Check the state of the controllers and their required interfaces using "
           "`ros2 control list_controllers -v` CLI to get more information.",  
           (  *ctrl_it ).c_str(   ) );
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT )
           {
           // remove controller that can not be activated from the activation request and step-back
           // iterator to correctly step to the next element in the list in the loop
           deactivate_request_.erase(  ctrl_it );
           --ctrl_it;
           }
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Aborting,   no controller is switched! (  ::STRICT switch )" );
           // reset all lists
           clear_requests(   );
           return controller_interface::return_type::ERROR;
           }
           }
           }
          
           for (  const auto & controller : controllers )
           {
           auto to_chained_mode_list_it = std::find(  
           to_chained_mode_request_.begin(   ),   to_chained_mode_request_.end(   ),   controller.info.name );
           bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(   );
          
           auto from_chained_mode_list_it = std::find(  
           from_chained_mode_request_.begin(   ),   from_chained_mode_request_.end(   ),   controller.info.name );
           bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(   );
          
           auto deactivate_list_it =
           std::find(  deactivate_request_.begin(   ),   deactivate_request_.end(   ),   controller.info.name );
           bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(   );
          
           const bool is_active = is_controller_active(  *controller.c );
           const bool is_inactive = is_controller_inactive(  *controller.c );
          
           // restart controllers that need to switch their 'chained mode' - add to (  de )activate lists
           if (  in_to_chained_mode_list || in_from_chained_mode_list )
           {
           if (  is_active && !in_deactivate_list )
           {
           deactivate_request_.push_back(  controller.info.name );
           activate_request_.push_back(  controller.info.name );
           }
           }
          
           // get pointers to places in deactivate and activate lists (  (  de )activate lists have changed )
           deactivate_list_it =
           std::find(  deactivate_request_.begin(   ),   deactivate_request_.end(   ),   controller.info.name );
           in_deactivate_list = deactivate_list_it != deactivate_request_.end(   );
          
           auto activate_list_it =
           std::find(  activate_request_.begin(   ),   activate_request_.end(   ),   controller.info.name );
           bool in_activate_list = activate_list_it != activate_request_.end(   );
          
           auto handle_conflict = [&](  const std::string & msg )
           {
           if (  strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
           {
           RCLCPP_ERROR(  get_logger(   ),   "%s",   msg.c_str(   ) );
           deactivate_request_.clear(   );
           deactivate_command_interface_request_.clear(   );
           activate_request_.clear(   );
           activate_command_interface_request_.clear(   );
           to_chained_mode_request_.clear(   );
           from_chained_mode_request_.clear(   );
           return controller_interface::return_type::ERROR;
           }
           RCLCPP_WARN(  get_logger(   ),   "%s",   msg.c_str(   ) );
           return controller_interface::return_type::OK;
           };
          
           // check for double stop
           if (  !is_active && in_deactivate_list )
           {
           auto ret = handle_conflict(  
           "Could not deactivate controller '" + controller.info.name + "' since it is not active" );
           if (  ret != controller_interface::return_type::OK )
           {
           return ret;
           }
           in_deactivate_list = false;
           deactivate_request_.erase(  deactivate_list_it );
           }
          
           // check for doubled activation
           if (  is_active && !in_deactivate_list && in_activate_list )
           {
           auto ret = handle_conflict(  
           "Could not activate controller '" + controller.info.name + "' since it is already active" );
           if (  ret != controller_interface::return_type::OK )
           {
           return ret;
           }
           in_activate_list = false;
           activate_request_.erase(  activate_list_it );
           }
          
           // check for illegal activation of an unconfigured/finalized controller
           if (  !is_inactive && !in_deactivate_list && in_activate_list )
           {
           auto ret = handle_conflict(  
           "Could not activate controller '" + controller.info.name +
           "' since it is not in inactive state" );
           if (  ret != controller_interface::return_type::OK )
           {
           return ret;
           }
           in_activate_list = false;
           activate_request_.erase(  activate_list_it );
           }
          
           const auto extract_interfaces_for_controller =
           [this](  const ControllerSpec controller,   std::vector<std::string> & request_interface_list )
           {
           auto command_interface_config = controller.c->command_interface_configuration(   );
           std::vector<std::string> command_interface_names = {};
           if (  command_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           command_interface_names = resource_manager_->available_command_interfaces(   );
           }
           if (  
           command_interface_config.type ==
           controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           command_interface_names = command_interface_config.names;
           }
           request_interface_list.insert(  
           request_interface_list.end(   ),   command_interface_names.begin(   ),  
           command_interface_names.end(   ) );
           };
          
           if (  in_activate_list )
           {
           extract_interfaces_for_controller(  controller,   activate_command_interface_request_ );
           }
           if (  in_deactivate_list )
           {
           extract_interfaces_for_controller(  controller,   deactivate_command_interface_request_ );
           }
           }
          
           if (  activate_request_.empty(   ) && deactivate_request_.empty(   ) )
           {
           RCLCPP_INFO(  get_logger(   ),   "Empty activate and deactivate list,   not requesting switch" );
           clear_requests(   );
           return controller_interface::return_type::OK;
           }
          
           if (  
           !activate_command_interface_request_.empty(   ) || !deactivate_command_interface_request_.empty(   ) )
           {
           if (  !resource_manager_->prepare_command_mode_switch(  
           activate_command_interface_request_,   deactivate_command_interface_request_ ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Could not switch controllers since prepare command mode switch was rejected." );
           clear_requests(   );
           return controller_interface::return_type::ERROR;
           }
           }
           // start the atomic controller switching
           switch_params_.strictness = strictness;
           switch_params_.activate_asap = activate_asap;
           switch_params_.init_time = rclcpp::Clock(   ).now(   );
           switch_params_.timeout = timeout;
           switch_params_.do_switch = true;
          
           // wait until switch is finished
           RCLCPP_DEBUG(  get_logger(   ),   "Requested atomic controller switch from realtime loop" );
           while (  rclcpp::ok(   ) && switch_params_.do_switch )
           {
           if (  !rclcpp::ok(   ) )
           {
           return controller_interface::return_type::ERROR;
           }
           std::this_thread::sleep_for(  std::chrono::microseconds(  100 ) );
           }
          
           // copy the controllers spec from the used to the unused list
           std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(  guard );
           to = controllers;
          
           // update the claimed interface controller info
           for (  auto & controller : to )
           {
           if (  is_controller_active(  controller.c ) )
           {
           auto command_interface_config = controller.c->command_interface_configuration(   );
           if (  command_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(   );
           }
           if (  
           command_interface_config.type ==
           controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           controller.info.claimed_interfaces = command_interface_config.names;
           }
           }
           else
           {
           controller.info.claimed_interfaces.clear(   );
           }
           }
           // switch lists
           rt_controllers_wrapper_.switch_updated_list(  guard );
           // clear unused list
           rt_controllers_wrapper_.get_unused_list(  guard ).clear(   );
          
           clear_requests(   );
          
           RCLCPP_DEBUG(  get_logger(   ),   "Successfully switched controllers" );
           return controller_interface::return_type::OK;
          }
          
          controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl(  
           const ControllerSpec & controller )
          {
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
          
           std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(  guard );
           const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(  guard );
          
           // Copy all controllers from the 'from' list to the 'to' list
           to = from;
          
           auto found_it = std::find_if(  
           to.begin(   ),   to.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   controller.info.name ) );
           // Checks that we're not duplicating controllers
           if (  found_it != to.end(   ) )
           {
           to.clear(   );
           RCLCPP_ERROR(  
           get_logger(   ),   "A controller named '%s' was already loaded inside the controller manager",  
           controller.info.name.c_str(   ) );
           return nullptr;
           }
          
           if (  
           controller.c->init(  controller.info.name,   get_namespace(   ) ) ==
           controller_interface::return_type::ERROR )
           {
           to.clear(   );
           RCLCPP_ERROR(  
           get_logger(   ),   "Could not initialize the controller named '%s'",   controller.info.name.c_str(   ) );
           return nullptr;
           }
          
           // ensure controller's `use_sim_time` parameter matches controller_manager's
           const rclcpp::Parameter use_sim_time = this->get_parameter(  "use_sim_time" );
           if (  use_sim_time.as_bool(   ) )
           {
           RCLCPP_INFO(  
           get_logger(   ),  
           "Setting use_sim_time=True for %s to match controller manager "
           "(  see ros2_control#325 for details )",  
           controller.info.name.c_str(   ) );
           controller.c->get_node(   )->set_parameter(  use_sim_time );
           }
           executor_->add_node(  controller.c->get_node(   )->get_node_base_interface(   ) );
           to.emplace_back(  controller );
          
           // Destroys the old controllers list when the realtime thread is finished with it.
           RCLCPP_DEBUG(  get_logger(   ),   "Realtime switches over to new controller list" );
           rt_controllers_wrapper_.switch_updated_list(  guard );
           RCLCPP_DEBUG(  get_logger(   ),   "Destruct controller" );
           std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list(  guard );
           new_unused_list.clear(   );
           RCLCPP_DEBUG(  get_logger(   ),   "Destruct controller finished" );
          
           return to.back(   ).c;
          }
          
          void ControllerManager::manage_switch(   )
          {
           // Ask hardware interfaces to change mode
           if (  !resource_manager_->perform_command_mode_switch(  
           activate_command_interface_request_,   deactivate_command_interface_request_ ) )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Error while performing mode switch." );
           }
          
           deactivate_controllers(   );
          
           switch_chained_mode(  to_chained_mode_request_,   true );
           switch_chained_mode(  from_chained_mode_request_,   false );
          
           // activate controllers once the switch is fully complete
           if (  !switch_params_.activate_asap )
           {
           activate_controllers(   );
           }
           else
           {
           // activate controllers as soon as their required joints are done switching
           activate_controllers_asap(   );
           }
          
           // TODO(  destogl ): move here "do_switch = false"
          }
          
          void ControllerManager::deactivate_controllers(   )
          {
           std::vector<ControllerSpec> & rt_controller_list =
           rt_controllers_wrapper_.update_and_get_used_by_rt_list(   );
           // stop controllers
           for (  const auto & request : deactivate_request_ )
           {
           auto found_it = std::find_if(  
           rt_controller_list.begin(   ),   rt_controller_list.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   request ) );
           if (  found_it == rt_controller_list.end(   ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Got request to stop controller '%s' but it is not in the realtime controller list",  
           request.c_str(   ) );
           continue;
           }
           auto controller = found_it->c;
           if (  is_controller_active(  *controller ) )
           {
           const auto new_state = controller->get_node(   )->deactivate(   );
           controller->release_interfaces(   );
           if (  new_state.id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "After deactivating,   controller '%s' is in state '%s',   expected Inactive",  
           request.c_str(   ),   new_state.label(   ).c_str(   ) );
           }
           }
           }
          }
          
          void ControllerManager::switch_chained_mode(  
           const std::vector<std::string> & chained_mode_switch_list,   bool to_chained_mode )
          {
           std::vector<ControllerSpec> & rt_controller_list =
           rt_controllers_wrapper_.update_and_get_used_by_rt_list(   );
          
           for (  const auto & request : chained_mode_switch_list )
           {
           auto found_it = std::find_if(  
           rt_controller_list.begin(   ),   rt_controller_list.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   request ) );
           if (  found_it == rt_controller_list.end(   ) )
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Got request to turn %s chained mode for controller '%s',   but controller is not in the "
           "realtime controller list. (  This should never happen! )",  
           (  to_chained_mode ? "ON" : "OFF" ),   request.c_str(   ) );
           continue;
           }
           auto controller = found_it->c;
           if (  !is_controller_active(  *controller ) )
           {
           if (  controller->set_chained_mode(  to_chained_mode ) )
           {
           if (  to_chained_mode )
           {
           resource_manager_->make_controller_reference_interfaces_available(  request );
           }
           else
           {
           resource_manager_->make_controller_reference_interfaces_unavailable(  request );
           }
           }
           else
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Got request to turn %s chained mode for controller '%s',   but controller refused to do "
           "it! The control will probably not work as expected. Try to restart all controllers. "
           "If "
           "the error persist check controllers' individual configuration.",  
           (  to_chained_mode ? "ON" : "OFF" ),   request.c_str(   ) );
           }
           }
           else
           {
           RCLCPP_FATAL(  
           get_logger(   ),  
           "Got request to turn %s chained mode for controller '%s',   but this can not happen if "
           "controller is in '%s' state. (  This should never happen! )",  
           (  to_chained_mode ? "ON" : "OFF" ),   request.c_str(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           }
           }
          }
          
          void ControllerManager::activate_controllers(   )
          {
           std::vector<ControllerSpec> & rt_controller_list =
           rt_controllers_wrapper_.update_and_get_used_by_rt_list(   );
           for (  const auto & request : activate_request_ )
           {
           auto found_it = std::find_if(  
           rt_controller_list.begin(   ),   rt_controller_list.end(   ),  
           std::bind(  controller_name_compare,   std::placeholders::_1,   request ) );
           if (  found_it == rt_controller_list.end(   ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Got request to activate controller '%s' but it is not in the realtime controller list",  
           request.c_str(   ) );
           continue;
           }
           auto controller = found_it->c;
          
           bool assignment_successful = true;
           // assign command interfaces to the controller
           auto command_interface_config = controller->command_interface_configuration(   );
           // default to controller_interface::configuration_type::NONE
           std::vector<std::string> command_interface_names = {};
           if (  command_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           command_interface_names = resource_manager_->available_command_interfaces(   );
           }
           if (  
           command_interface_config.type ==
           controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           command_interface_names = command_interface_config.names;
           }
           std::vector<hardware_interface::LoanedCommandInterface> command_loans;
           command_loans.reserve(  command_interface_names.size(   ) );
           for (  const auto & command_interface : command_interface_names )
           {
           if (  resource_manager_->command_interface_is_claimed(  command_interface ) )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Resource conflict for controller '%s'. Command interface '%s' is already claimed.",  
           request.c_str(   ),   command_interface.c_str(   ) );
           assignment_successful = false;
           break;
           }
           try
           {
           command_loans.emplace_back(  resource_manager_->claim_command_interface(  command_interface ) );
           }
           catch (  const std::exception & e )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Can't activate controller '%s': %s",   request.c_str(   ),   e.what(   ) );
           assignment_successful = false;
           break;
           }
           }
           // something went wrong during command interfaces,   go skip the controller
           if (  !assignment_successful )
           {
           continue;
           }
          
           // assign state interfaces to the controller
           auto state_interface_config = controller->state_interface_configuration(   );
           // default to controller_interface::configuration_type::NONE
           std::vector<std::string> state_interface_names = {};
           if (  state_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           state_interface_names = resource_manager_->available_state_interfaces(   );
           }
           if (  
           state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           state_interface_names = state_interface_config.names;
           }
           std::vector<hardware_interface::LoanedStateInterface> state_loans;
           state_loans.reserve(  state_interface_names.size(   ) );
           for (  const auto & state_interface : state_interface_names )
           {
           try
           {
           state_loans.emplace_back(  resource_manager_->claim_state_interface(  state_interface ) );
           }
           catch (  const std::exception & e )
           {
           RCLCPP_ERROR(  get_logger(   ),   "Can't activate controller '%s': %s",   request.c_str(   ),   e.what(   ) );
           assignment_successful = false;
           break;
           }
           }
           // something went wrong during state interfaces,   go skip the controller
           if (  !assignment_successful )
           {
           continue;
           }
           controller->assign_interfaces(  std::move(  command_loans ),   std::move(  state_loans ) );
          
           const auto new_state = controller->get_node(   )->activate(   );
           if (  new_state.id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "After activating,   controller '%s' is in state '%s',   expected Active",  
           controller->get_node(   )->get_name(   ),   new_state.label(   ).c_str(   ) );
           }
           }
           // All controllers activated,   switching done
           switch_params_.do_switch = false;
          }
          
          void ControllerManager::activate_controllers_asap(   )
          {
           // https://github.com/ros-controls/ros2_control/issues/263
           activate_controllers(   );
          }
          
          void ControllerManager::list_controllers_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,  
           std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  get_logger(   ),   "list controller service called" );
           std::lock_guard<std::mutex> services_guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "list controller service locked" );
          
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
           const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(  guard );
           response->controller.resize(  controllers.size(   ) );
          
           for (  size_t i = 0; i < controllers.size(   ); ++i )
           {
           controller_manager_msgs::msg::ControllerState & cs = response->controller[i];
           cs.name = controllers[i].info.name;
           cs.type = controllers[i].info.type;
           cs.claimed_interfaces = controllers[i].info.claimed_interfaces;
           cs.state = controllers[i].c->get_state(   ).label(   );
          
           // Get information about interfaces if controller are in 'inactive' or 'active' state
           if (  is_controller_active(  controllers[i].c ) || is_controller_inactive(  controllers[i].c ) )
           {
           auto command_interface_config = controllers[i].c->command_interface_configuration(   );
           if (  command_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           cs.required_command_interfaces = resource_manager_->command_interface_keys(   );
           }
           else if (  
           command_interface_config.type ==
           controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           cs.required_command_interfaces = command_interface_config.names;
           }
          
           auto state_interface_config = controllers[i].c->state_interface_configuration(   );
           if (  state_interface_config.type == controller_interface::interface_configuration_type::ALL )
           {
           cs.required_state_interfaces = resource_manager_->state_interface_keys(   );
           }
           else if (  
           state_interface_config.type ==
           controller_interface::interface_configuration_type::INDIVIDUAL )
           {
           cs.required_state_interfaces = state_interface_config.names;
           }
           }
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "list controller service finished" );
          }
          
          void ControllerManager::list_controller_types_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,  
           std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  get_logger(   ),   "list types service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "list types service locked" );
          
           auto cur_types = loader_->getDeclaredClasses(   );
           for (  const auto & cur_type : cur_types )
           {
           response->types.push_back(  cur_type );
           response->base_classes.push_back(  kControllerInterfaceClassName );
           RCLCPP_DEBUG(  get_logger(   ),   "%s",   cur_type.c_str(   ) );
           }
           cur_types = chainable_loader_->getDeclaredClasses(   );
           for (  const auto & cur_type : cur_types )
           {
           response->types.push_back(  cur_type );
           response->base_classes.push_back(  kChainableControllerInterfaceClassName );
           RCLCPP_DEBUG(  get_logger(   ),   "%s",   cur_type.c_str(   ) );
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "list types service finished" );
          }
          
          void ControllerManager::load_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  get_logger(   ),   "loading service called for controller '%s' ",   request->name.c_str(   ) );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "loading service locked" );
          
           response->ok = load_controller(  request->name ).get(   ) != nullptr;
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "loading service finished for controller '%s' ",   request->name.c_str(   ) );
          }
          
          void ControllerManager::configure_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  
           get_logger(   ),   "configuring service called for controller '%s' ",   request->name.c_str(   ) );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "configuring service locked" );
          
           response->ok = configure_controller(  request->name ) == controller_interface::return_type::OK;
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "configuring service finished for controller '%s' ",   request->name.c_str(   ) );
          }
          
          void ControllerManager::reload_controller_libraries_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  get_logger(   ),   "reload libraries service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "reload libraries service locked" );
          
           // only reload libraries if no controllers are active
           std::vector<std::string> loaded_controllers,   active_controllers;
           loaded_controllers = get_controller_names(   );
           {
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
           for (  const auto & controller : rt_controllers_wrapper_.get_updated_list(  guard ) )
           {
           if (  is_controller_active(  *controller.c ) )
           {
           active_controllers.push_back(  controller.info.name );
           }
           }
           }
           if (  !active_controllers.empty(   ) && !request->force_kill )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Controller manager: Cannot reload controller libraries because"
           " there are still %i active controllers",  
           (  int )active_controllers.size(   ) );
           response->ok = false;
           return;
           }
          
           // stop active controllers if requested
           if (  !loaded_controllers.empty(   ) )
           {
           RCLCPP_INFO(  get_logger(   ),   "Controller manager: Stopping all active controllers" );
           std::vector<std::string> empty;
           if (  
           switch_controller(  
           empty,   active_controllers,  
           controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ) !=
           controller_interface::return_type::OK )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Controller manager: Cannot reload controller libraries because failed to stop "
           "active controllers" );
           response->ok = false;
           return;
           }
           for (  const auto & controller : loaded_controllers )
           {
           if (  unload_controller(  controller ) != controller_interface::return_type::OK )
           {
           RCLCPP_ERROR(  
           get_logger(   ),  
           "Controller manager: Cannot reload controller libraries because "
           "failed to unload controller '%s'",  
           controller.c_str(   ) );
           response->ok = false;
           return;
           }
           }
           loaded_controllers = get_controller_names(   );
           }
           assert(  loaded_controllers.empty(   ) );
          
           // Force a reload on all the PluginLoaders (  internally,   this recreates the plugin loaders )
           loader_ = std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(  
           kControllerInterfaceNamespace,   kControllerInterfaceClassName );
           chainable_loader_ =
           std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(  
           kControllerInterfaceNamespace,   kChainableControllerInterfaceClassName );
           RCLCPP_INFO(  
           get_logger(   ),   "Controller manager: reloaded controller libraries for '%s'",  
           kControllerInterfaceNamespace );
          
           response->ok = true;
          
           RCLCPP_DEBUG(  get_logger(   ),   "reload libraries service finished" );
          }
          
          void ControllerManager::switch_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  get_logger(   ),   "switching service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "switching service locked" );
          
           // response->ok = switch_controller(  
           // request->activate_controllers,   request->deactivate_controllers,   request->strictness,  
           // request->activate_asap,   request->timeout ) == controller_interface::return_type::OK;
           // TODO(  destogl ): remove this after deprecated fields are removed from service and use the
           // commented three lines above
           // BEGIN: remove when deprecated removed
           auto activate_controllers = request->activate_controllers;
           auto deactivate_controllers = request->deactivate_controllers;
          
           if (  !request->start_controllers.empty(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "'start_controllers' field is deprecated,   use 'activate_controllers' field instead!" );
           activate_controllers.insert(  
           activate_controllers.end(   ),   request->start_controllers.begin(   ),  
           request->start_controllers.end(   ) );
           }
           if (  !request->stop_controllers.empty(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "'stop_controllers' field is deprecated,   use 'deactivate_controllers' field instead!" );
           deactivate_controllers.insert(  
           deactivate_controllers.end(   ),   request->stop_controllers.begin(   ),  
           request->stop_controllers.end(   ) );
           }
          
           auto activate_asap = request->activate_asap;
           if (  request->start_asap )
           {
           RCLCPP_WARN(  
           get_logger(   ),   "'start_asap' field is deprecated,   use 'activate_asap' field instead!" );
           activate_asap = request->start_asap;
           }
          
           response->ok = switch_controller(  
           activate_controllers,   deactivate_controllers,   request->strictness,   activate_asap,  
           request->timeout ) == controller_interface::return_type::OK;
           // END: remove when deprecated removed
          
           RCLCPP_DEBUG(  get_logger(   ),   "switching service finished" );
          }
          
          void ControllerManager::unload_controller_service_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response )
          {
           // lock services
           RCLCPP_DEBUG(  
           get_logger(   ),   "unloading service called for controller '%s' ",   request->name.c_str(   ) );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "unloading service locked" );
          
           response->ok = unload_controller(  request->name ) == controller_interface::return_type::OK;
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "unloading service finished for controller '%s' ",   request->name.c_str(   ) );
          }
          
          void ControllerManager::list_hardware_components_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request>,  
           std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware components service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware components service locked" );
          
           auto hw_components_info = resource_manager_->get_components_status(   );
          
           response->component.reserve(  hw_components_info.size(   ) );
          
           for (  const auto & [component_name,   component_info] : hw_components_info )
           {
           auto component = controller_manager_msgs::msg::HardwareComponentState(   );
           component.name = component_name;
           component.type = component_info.type;
           component.class_type = component_info.class_type;
           component.state.id = component_info.state.id(   );
           component.state.label = component_info.state.label(   );
          
           component.command_interfaces.reserve(  component_info.command_interfaces.size(   ) );
           for (  const auto & interface : component_info.command_interfaces )
           {
           controller_manager_msgs::msg::HardwareInterface hwi;
           hwi.name = interface;
           hwi.is_available = resource_manager_->command_interface_is_available(  interface );
           hwi.is_claimed = resource_manager_->command_interface_is_claimed(  interface );
           component.command_interfaces.push_back(  hwi );
           }
          
           component.state_interfaces.reserve(  component_info.state_interfaces.size(   ) );
           for (  const auto & interface : component_info.state_interfaces )
           {
           controller_manager_msgs::msg::HardwareInterface hwi;
           hwi.name = interface;
           hwi.is_available = resource_manager_->state_interface_is_available(  interface );
           hwi.is_claimed = false;
           component.state_interfaces.push_back(  hwi );
           }
          
           response->component.push_back(  component );
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware components service finished" );
          }
          
          void ControllerManager::list_hardware_interfaces_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request>,  
           std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware interfaces service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware interfaces service locked" );
          
           auto state_interface_names = resource_manager_->state_interface_keys(   );
           for (  const auto & state_interface_name : state_interface_names )
           {
           controller_manager_msgs::msg::HardwareInterface hwi;
           hwi.name = state_interface_name;
           hwi.is_available = resource_manager_->state_interface_is_available(  state_interface_name );
           hwi.is_claimed = false;
           response->state_interfaces.push_back(  hwi );
           }
           auto command_interface_names = resource_manager_->command_interface_keys(   );
           for (  const auto & command_interface_name : command_interface_names )
           {
           controller_manager_msgs::msg::HardwareInterface hwi;
           hwi.name = command_interface_name;
           hwi.is_available = resource_manager_->command_interface_is_available(  command_interface_name );
           hwi.is_claimed = resource_manager_->command_interface_is_claimed(  command_interface_name );
           response->command_interfaces.push_back(  hwi );
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "list hardware interfaces service finished" );
          }
          
          void ControllerManager::set_hardware_component_state_srv_cb(  
           const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,  
           std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response )
          {
           RCLCPP_DEBUG(  get_logger(   ),   "set hardware component state service called" );
           std::lock_guard<std::mutex> guard(  services_lock_ );
           RCLCPP_DEBUG(  get_logger(   ),   "set hardware component state service locked" );
          
           RCLCPP_DEBUG(  get_logger(   ),   "set hardware component state '%s'",   request->name.c_str(   ) );
          
           auto hw_components_info = resource_manager_->get_components_status(   );
           if (  hw_components_info.find(  request->name ) != hw_components_info.end(   ) )
           {
           rclcpp_lifecycle::State target_state(  
           request->target_state.id,  
           // the ternary operator is needed because label in State constructor cannot be an empty string
           request->target_state.label.empty(   ) ? "-" : request->target_state.label );
           response->ok =
           (  resource_manager_->set_component_state(  request->name,   target_state ) ==
           hardware_interface::return_type::OK );
           hw_components_info = resource_manager_->get_components_status(   );
           response->state.id = hw_components_info[request->name].state.id(   );
           response->state.label = hw_components_info[request->name].state.label(   );
           }
           else
           {
           RCLCPP_ERROR(  
           get_logger(   ),   "hardware component with name '%s' does not exist",   request->name.c_str(   ) );
           response->ok = false;
           }
          
           RCLCPP_DEBUG(  get_logger(   ),   "set hardware component state service finished" );
          }
          
          std::vector<std::string> ControllerManager::get_controller_names(   )
          {
           std::vector<std::string> names;
          
           // lock controllers
           std::lock_guard<std::recursive_mutex> guard(  rt_controllers_wrapper_.controllers_lock_ );
           for (  const auto & controller : rt_controllers_wrapper_.get_updated_list(  guard ) )
           {
           names.push_back(  controller.info.name );
           }
           return names;
          }
          
          void ControllerManager::read(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           resource_manager_->read(  time,   period );
          }
          
          controller_interface::return_type ControllerManager::update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           std::vector<ControllerSpec> & rt_controller_list =
           rt_controllers_wrapper_.update_and_get_used_by_rt_list(   );
          
           auto ret = controller_interface::return_type::OK;
           ++update_loop_counter_;
           update_loop_counter_ %= update_rate_;
          
           for (  auto loaded_controller : rt_controller_list )
           {
           // TODO(  v-lopez ) we could cache this information
           // https://github.com/ros-controls/ros2_control/issues/153
           if (  is_controller_active(  *loaded_controller.c ) )
           {
           const auto controller_update_rate = loaded_controller.c->get_update_rate(   );
          
           bool controller_go =
           controller_update_rate == 0 || (  (  update_loop_counter_ % controller_update_rate ) == 0 );
           RCLCPP_DEBUG(  
           get_logger(   ),   "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",  
           update_loop_counter_,   controller_go ? "True" : "False",  
           loaded_controller.info.name.c_str(   ) );
          
           if (  controller_go )
           {
           auto controller_ret = loaded_controller.c->update(  
           time,   (  controller_update_rate != update_rate_ && controller_update_rate != 0 )
           ? rclcpp::Duration::from_seconds(  1.0 / controller_update_rate )
           : period );
          
           if (  controller_ret != controller_interface::return_type::OK )
           {
           ret = controller_ret;
           }
           }
           }
           }
          
           // there are controllers to (  de )activate
           if (  switch_params_.do_switch )
           {
           manage_switch(   );
           }
          
           return ret;
          }
          
          void ControllerManager::write(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           resource_manager_->write(  time,   period );
          }
          
          std::vector<ControllerSpec> &
          ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list(   )
          {
           used_by_realtime_controllers_index_ = updated_controllers_index_;
           return controllers_lists_[used_by_realtime_controllers_index_];
          }
          
          std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_unused_list(  
           const std::lock_guard<std::recursive_mutex> & )
          {
           if (  !controllers_lock_.try_lock(   ) )
           {
           throw std::runtime_error(  "controllers_lock_ not owned by thread" );
           }
           controllers_lock_.unlock(   );
           // Get the index to the outdated controller list
           int free_controllers_list = get_other_list(  updated_controllers_index_ );
          
           // Wait until the outdated controller list is not being used by the realtime thread
           wait_until_rt_not_using(  free_controllers_list );
           return controllers_lists_[free_controllers_list];
          }
          
          const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(  
           const std::lock_guard<std::recursive_mutex> & ) const
          {
           if (  !controllers_lock_.try_lock(   ) )
           {
           throw std::runtime_error(  "controllers_lock_ not owned by thread" );
           }
           controllers_lock_.unlock(   );
           return controllers_lists_[updated_controllers_index_];
          }
          
          void ControllerManager::RTControllerListWrapper::switch_updated_list(  
           const std::lock_guard<std::recursive_mutex> & )
          {
           if (  !controllers_lock_.try_lock(   ) )
           {
           throw std::runtime_error(  "controllers_lock_ not owned by thread" );
           }
           controllers_lock_.unlock(   );
           int former_current_controllers_list_ = updated_controllers_index_;
           updated_controllers_index_ = get_other_list(  former_current_controllers_list_ );
           wait_until_rt_not_using(  former_current_controllers_list_ );
          }
          
          int ControllerManager::RTControllerListWrapper::get_other_list(  int index ) const
          {
           return (  index + 1 ) % 2;
          }
          
          void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(  
           int index,   std::chrono::microseconds sleep_period ) const
          {
           while (  used_by_realtime_controllers_index_ == index )
           {
           if (  !rclcpp::ok(   ) )
           {
           throw std::runtime_error(  "rclcpp interrupted" );
           }
           std::this_thread::sleep_for(  sleep_period );
           }
          }
          
          unsigned int ControllerManager::get_update_rate(   ) const { return update_rate_; }
          
          void ControllerManager::propagate_deactivation_of_chained_mode(  
           const std::vector<ControllerSpec> & controllers )
          {
           for (  const auto & controller : controllers )
           {
           // get pointers to places in deactivate and activate lists (  (  de )activate lists have changed )
           auto deactivate_list_it =
           std::find(  deactivate_request_.begin(   ),   deactivate_request_.end(   ),   controller.info.name );
          
           if (  deactivate_list_it != deactivate_request_.end(   ) )
           {
           // if controller is not active then skip adding following-controllers to "from" chained mode
           // request
           if (  !is_controller_active(  controller.c ) )
           {
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Controller with name '%s' can not be deactivated since is not active. "
           "The controller will be removed from the list later."
           "Skipping adding following controllers to 'from' chained mode request.",  
           controller.info.name.c_str(   ) );
           break;
           }
          
           for (  const auto & cmd_itf_name : controller.c->command_interface_configuration(   ).names )
           {
           // controller that 'cmd_tf_name' belongs to
           ControllersListIterator following_ctrl_it;
           if (  command_interface_is_reference_interface_of_controller(  
           cmd_itf_name,   controllers,   following_ctrl_it ) )
           {
           // currently iterated "controller" is preceding controller --> add following controller
           // with matching interface name to "from" chained mode list (  if not already in it )
           if (  
           std::find(  
           from_chained_mode_request_.begin(   ),   from_chained_mode_request_.end(   ),  
           following_ctrl_it->info.name ) == from_chained_mode_request_.end(   ) )
           {
           from_chained_mode_request_.push_back(  following_ctrl_it->info.name );
           RCLCPP_DEBUG(  
           get_logger(   ),   "Adding controller '%s' in 'from chained mode' request.",  
           following_ctrl_it->info.name.c_str(   ) );
           }
           }
           }
           }
           }
          }
          
          controller_interface::return_type ControllerManager::check_following_controllers_for_activate(  
           const std::vector<ControllerSpec> & controllers,   int strictness,  
           const ControllersListIterator controller_it )
          {
           // we assume that the controller exists is checked in advance
           RCLCPP_DEBUG(  
           get_logger(   ),   "Checking following controllers of preceding controller with name '%s'.",  
           controller_it->info.name.c_str(   ) );
          
           for (  const auto & cmd_itf_name : controller_it->c->command_interface_configuration(   ).names )
           {
           ControllersListIterator following_ctrl_it;
           // Check if interface if reference interface and following controller exist.
           if (  !command_interface_is_reference_interface_of_controller(  
           cmd_itf_name,   controllers,   following_ctrl_it ) )
           {
           continue;
           }
           // TODO(  destogl ): performance of this code could be optimized by adding additional lists with
           // controllers that cache if the check has failed and has succeeded. Then the following would be
           // done only once per controller,   otherwise in complex scenarios the same controller is checked
           // multiple times
          
           // check that all following controllers exits,   are either: activated,   will be activated,   or
           // will not be deactivated
           RCLCPP_DEBUG(  
           get_logger(   ),   "Checking following controller with name '%s'.",  
           following_ctrl_it->info.name.c_str(   ) );
          
           // check if following controller is chainable
           if (  !following_ctrl_it->c->is_chainable(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "No reference interface '%s' exist,   since the following controller with name '%s' "
           "is not chainable.",  
           cmd_itf_name.c_str(   ),   following_ctrl_it->info.name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           if (  is_controller_active(  following_ctrl_it->c ) )
           {
           // will following controller be deactivated?
           if (  
           std::find(  
           deactivate_request_.begin(   ),   deactivate_request_.end(   ),   following_ctrl_it->info.name ) !=
           deactivate_request_.end(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),   "The following controller with name '%s' will be deactivated.",  
           following_ctrl_it->info.name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           }
           // check if following controller will not be activated
           else if (  
           std::find(  activate_request_.begin(   ),   activate_request_.end(   ),   following_ctrl_it->info.name ) ==
           activate_request_.end(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "The following controller with name '%s' is not active and will not be activated.",  
           following_ctrl_it->info.name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           // Trigger recursion to check all the following controllers only if they are OK,   add this
           // controller update chained mode requests
           if (  
           check_following_controllers_for_activate(  controllers,   strictness,   following_ctrl_it ) ==
           controller_interface::return_type::ERROR )
           {
           return controller_interface::return_type::ERROR;
           }
          
           // TODO(  destogl ): this should be discussed how to it the best - just a placeholder for now
           // else if (  strictness ==
           // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN )
           // {
           // // insert to the begin of activate request list to be activated before preceding controller
           // activate_request_.insert(  activate_request_.begin(   ),   following_ctrl_name );
           // }
           if (  !following_ctrl_it->c->is_in_chained_mode(   ) )
           {
           auto found_it = std::find(  
           to_chained_mode_request_.begin(   ),   to_chained_mode_request_.end(   ),  
           following_ctrl_it->info.name );
           if (  found_it == to_chained_mode_request_.end(   ) )
           {
           to_chained_mode_request_.push_back(  following_ctrl_it->info.name );
           RCLCPP_DEBUG(  
           get_logger(   ),   "Adding controller '%s' in 'to chained mode' request.",  
           following_ctrl_it->info.name.c_str(   ) );
           }
           }
           else
           {
           // Check if following controller is in 'from' chained mode list and remove it,   if so
           auto found_it = std::find(  
           from_chained_mode_request_.begin(   ),   from_chained_mode_request_.end(   ),  
           following_ctrl_it->info.name );
           if (  found_it != from_chained_mode_request_.end(   ) )
           {
           from_chained_mode_request_.erase(  found_it );
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Removing controller '%s' in 'from chained mode' request because it "
           "should stay in chained mode.",  
           following_ctrl_it->info.name.c_str(   ) );
           }
           }
           }
           return controller_interface::return_type::OK;
          };
          
          controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate(  
           const std::vector<ControllerSpec> & controllers,   int /*strictness*/,  
           const ControllersListIterator controller_it )
          {
           // if not chainable no need for any checks
           if (  !controller_it->c->is_chainable(   ) )
           {
           return controller_interface::return_type::OK;
           }
          
           if (  !controller_it->c->is_in_chained_mode(   ) )
           {
           RCLCPP_DEBUG(  
           get_logger(   ),  
           "Controller with name '%s' is chainable but not in chained mode. "
           "No need to do any checks of preceding controllers when stopping it.",  
           controller_it->info.name.c_str(   ) );
           return controller_interface::return_type::OK;
           }
          
           RCLCPP_DEBUG(  
           get_logger(   ),   "Checking preceding controller of following controller with name '%s'.",  
           controller_it->info.name.c_str(   ) );
          
           for (  const auto & ref_itf_name :
           resource_manager_->get_controller_reference_interface_names(  controller_it->info.name ) )
           {
           std::vector<ControllersListIterator> preceding_controllers_using_ref_itf;
          
           // TODO(  destogl ): This data could be cached after configuring controller into a map for faster
           // access here
           for (  auto preceding_ctrl_it = controllers.begin(   ); preceding_ctrl_it != controllers.end(   );
           ++preceding_ctrl_it )
           {
           const auto preceding_ctrl_cmd_itfs =
           preceding_ctrl_it->c->command_interface_configuration(   ).names;
          
           // if controller is not preceding go the next one
           if (  
           std::find(  preceding_ctrl_cmd_itfs.begin(   ),   preceding_ctrl_cmd_itfs.end(   ),   ref_itf_name ) ==
           preceding_ctrl_cmd_itfs.end(   ) )
           {
           continue;
           }
          
           // check if preceding controller will be activated
           if (  
           is_controller_inactive(  preceding_ctrl_it->c ) &&
           std::find(  
           activate_request_.begin(   ),   activate_request_.end(   ),   preceding_ctrl_it->info.name ) !=
           activate_request_.end(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Could not deactivate controller with name '%s' because "
           "preceding controller with name '%s' will be activated. ",  
           controller_it->info.name.c_str(   ),   preceding_ctrl_it->info.name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           // check if preceding controller will not be deactivated
           else if (  
           is_controller_active(  preceding_ctrl_it->c ) &&
           std::find(  
           deactivate_request_.begin(   ),   deactivate_request_.end(   ),   preceding_ctrl_it->info.name ) ==
           deactivate_request_.end(   ) )
           {
           RCLCPP_WARN(  
           get_logger(   ),  
           "Could not deactivate controller with name '%s' because "
           "preceding controller with name '%s' is active and will not be deactivated.",  
           controller_it->info.name.c_str(   ),   preceding_ctrl_it->info.name.c_str(   ) );
           return controller_interface::return_type::ERROR;
           }
           // TODO(  destogl ): this should be discussed how to it the best - just a placeholder for now
           // else if (  
           // strictness ==
           // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN )
           // {
           // // insert to the begin of activate request list to be activated before preceding controller
           // activate_request_.insert(  activate_request_.begin(   ),   preceding_ctrl_name );
           // }
           }
           }
           return controller_interface::return_type::OK;
          };
          
          } // namespace controller_manager

ros2_control/controller_manager/src/realtime.cpp

       1  // Copyright 2022 PickNik Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "controller_manager/realtime.hpp"
          
          #include <sched.h>
          
          #include <cstring>
          #include <fstream>
          
          namespace controller_manager
          {
      24  bool has_realtime_kernel(   )
          {
           std::ifstream realtime_file(  "/sys/kernel/realtime",   std::ios::in );
           bool has_realtime = false;
           if (  realtime_file.is_open(   ) )
           {
           realtime_file >> has_realtime;
           }
           return has_realtime;
          }
          
      35  bool configure_sched_fifo(  int priority )
          {
           struct sched_param schedp;
           memset(  &schedp,   0,   sizeof(  schedp ) );
           schedp.sched_priority = priority;
           if (  sched_setscheduler(  0,   SCHED_FIFO,   &schedp ) )
           {
           return false;
           }
           return true;
          }
          
          } // namespace controller_manager

ros2_control/controller_manager/src/ros2_control_node.cpp

       1  // Copyright 2020 ROS2-Control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <algorithm>
          #include <chrono>
          #include <memory>
          #include <string>
          #include <thread>
          
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager/realtime.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          using namespace std::chrono_literals;
          
          namespace
          {
          // Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html
          // This value is used when configuring the main loop to use SCHED_FIFO scheduling
          // We use a midpoint RT priority to allow maximum flexibility to users
      32  int const kSchedPriority = 50;
          
          } // namespace
          
      36  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::MultiThreadedExecutor>(   );
           std::string manager_node_name = "controller_manager";
          
           auto cm = std::make_shared<controller_manager::ControllerManager>(  executor,   manager_node_name );
          
           RCLCPP_INFO(  cm->get_logger(   ),   "update rate is %d Hz",   cm->get_update_rate(   ) );
          
           std::thread cm_thread(  
           [cm](   )
           {
           if (  controller_manager::has_realtime_kernel(   ) )
           {
           if (  !controller_manager::configure_sched_fifo(  kSchedPriority ) )
           {
           RCLCPP_WARN(  cm->get_logger(   ),   "Could not enable FIFO RT scheduling policy" );
           }
           }
           else
           {
           RCLCPP_INFO(  cm->get_logger(   ),   "RT kernel is recommended for better performance" );
           }
          
           // for calculating sleep time
           auto const period = std::chrono::nanoseconds(  1'000'000'000 / cm->get_update_rate(   ) );
           std::chrono::system_clock::time_point next_iteration_time =
           std::chrono::system_clock::time_point(  std::chrono::nanoseconds(  cm->now(   ).nanoseconds(   ) ) );
          
           // for calculating the measured period of the loop
           rclcpp::Time previous_time = cm->now(   );
          
           while (  rclcpp::ok(   ) )
           {
           // calculate measured period
           auto const current_time = cm->now(   );
           auto const measured_period = current_time - previous_time;
           previous_time = current_time;
          
           // execute update loop
           cm->read(  cm->now(   ),   measured_period );
           cm->update(  cm->now(   ),   measured_period );
           cm->write(  cm->now(   ),   measured_period );
          
           // wait until we hit the end of the period
           next_iteration_time += period;
           std::this_thread::sleep_until(  next_iteration_time );
           }
           } );
          
           executor->add_node(  cm );
           executor->spin(   );
           cm_thread.join(   );
           rclcpp::shutdown(   );
           return 0;
          }

ros2_control/controller_manager/test/controller_manager_test_common.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef CONTROLLER_MANAGER_TEST_COMMON_HPP_
          #define CONTROLLER_MANAGER_TEST_COMMON_HPP_
          
          #include <gmock/gmock.h>
          #include <gtest/gtest.h>
          
          #include <chrono>
          #include <memory>
          #include <string>
          #include <thread>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_msgs/srv/switch_controller.hpp"
          
          #include "rclcpp/utilities.hpp"
          
          #include "ros2_control_test_assets/descriptions.hpp"
          #include "test_controller_failed_init/test_controller_failed_init.hpp"
          
          namespace
          {
      39  const auto TIME = rclcpp::Time(  0 );
          const auto PERIOD = rclcpp::Duration::from_seconds(  0.01 );
          const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
          const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
          const auto TEST_CM_NAME = "test_controller_manager";
          } // namespace
          // Strictness structure for parameterized tests - shared between different tests
          struct Strictness
          {
           int strictness = STRICT;
           controller_interface::return_type expected_return;
           unsigned int expected_counter;
          };
          Strictness strict{STRICT,   controller_interface::return_type::ERROR,   0u};
          Strictness best_effort{BEST_EFFORT,   controller_interface::return_type::OK,   1u};
          
          // Forward definition to avid compile error - defined at the end of the file
          template <typename CtrlMgr>
          class ControllerManagerRunner;
          
          template <typename CtrlMgr>
          class ControllerManagerFixture : public ::testing::Test
          {
          public:
           static void SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
           static void TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
           void SetUp(   )
           {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           cm_ = std::make_shared<CtrlMgr>(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf,   true,   true ),  
           executor_,   TEST_CM_NAME );
           run_updater_ = false;
           }
          
           void TearDown(   ) { stopCmUpdater(   ); }
          
           void startCmUpdater(   )
           {
           run_updater_ = true;
           updater_ = std::thread(  
           [&](  void ) -> void
           {
           while (  run_updater_ )
           {
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  10 ) );
           }
           } );
           }
          
           void stopCmUpdater(   )
           {
           if (  run_updater_ )
           {
           run_updater_ = false;
           updater_.join(   );
           }
           }
          
           void switch_test_controllers(  
           const std::vector<std::string> & start_controllers,  
           const std::vector<std::string> & stop_controllers,   const int strictness,  
           const std::future_status expected_future_status = std::future_status::timeout,  
           const controller_interface::return_type expected_return = controller_interface::return_type::OK )
           {
           // First activation not possible because controller not configured
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   strictness,   true,   rclcpp::Duration(  0,   0 ) );
          
           ASSERT_EQ(  expected_future_status,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner<CtrlMgr> cm_runner(  this );
           EXPECT_EQ(  expected_return,   switch_future.get(   ) );
           }
          
           std::shared_ptr<rclcpp::Executor> executor_;
           std::shared_ptr<CtrlMgr> cm_;
          
           std::thread updater_;
           bool run_updater_;
          };
          
          class TestControllerManagerSrvs
          : public ControllerManagerFixture<controller_manager::ControllerManager>
          {
          public:
           TestControllerManagerSrvs(   ) {}
          
           void SetUp(   ) override
           {
           ControllerManagerFixture::SetUp(   );
           SetUpSrvsCMExecutor(   );
           }
          
           void SetUpSrvsCMExecutor(   )
           {
           update_timer_ = cm_->create_wall_timer(  
           std::chrono::milliseconds(  10 ),  
           [&](   )
           {
           cm_->read(  TIME,   PERIOD );
           cm_->update(  TIME,   PERIOD );
           cm_->write(  TIME,   PERIOD );
           } );
          
           executor_->add_node(  cm_ );
          
           executor_spin_future_ = std::async(  std::launch::async,   [this](   ) -> void { executor_->spin(   ); } );
           // This sleep is needed to prevent a too fast test from ending before the
           // executor has began to spin,   which causes it to hang
           std::this_thread::sleep_for(  std::chrono::milliseconds(  50 ) );
           }
          
           // FIXME: This can be deleted!
           void TearDown(   ) override { executor_->cancel(   ); }
          
           template <typename T>
           std::shared_ptr<typename T::Response> call_service_and_wait(  
           rclcpp::Client<T> & client,   std::shared_ptr<typename T::Request> request,  
           rclcpp::Executor & service_executor,   bool update_controller_while_spinning = false )
           {
           EXPECT_TRUE(  client.wait_for_service(  std::chrono::milliseconds(  500 ) ) );
           auto result = client.async_send_request(  request );
           // Wait for the result.
           if (  update_controller_while_spinning )
           {
           while (  service_executor.spin_until_future_complete(  result,   std::chrono::milliseconds(  50 ) ) !=
           rclcpp::FutureReturnCode::SUCCESS )
           {
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           }
           }
           else
           {
           EXPECT_EQ(  
           service_executor.spin_until_future_complete(  result ),   rclcpp::FutureReturnCode::SUCCESS );
           }
           return result.get(   );
           }
          
          protected:
           rclcpp::TimerBase::SharedPtr update_timer_;
           std::future<void> executor_spin_future_;
          };
          
          template <typename CtrlMgr>
          class ControllerManagerRunner
          {
          public:
           explicit ControllerManagerRunner(  ControllerManagerFixture<CtrlMgr> * cmf ) : cmf_(  cmf )
           {
           cmf_->startCmUpdater(   );
           }
          
           ~ControllerManagerRunner(   ) { cmf_->stopCmUpdater(   ); }
          
           ControllerManagerFixture<CtrlMgr> * cmf_;
          };
          
          class ControllerMock : public controller_interface::ControllerInterface
          {
          public:
           MOCK_METHOD0(  update,   controller_interface::return_type(  void ) );
          };
          
          #endif // CONTROLLER_MANAGER_TEST_COMMON_HPP_

ros2_control/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

          // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_chainable_controller.hpp"
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace test_chainable_controller
          {
      25  TestChainableController::TestChainableController(   )
          : controller_interface::ChainableControllerInterface(   ),  
           cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE},  
      28   state_iface_cfg_{controller_interface::interface_configuration_type::NONE}
          {
          }
          
          controller_interface::InterfaceConfiguration
          TestChainableController::command_interface_configuration(   ) const
          {
           if (  
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           return cmd_iface_cfg_;
           }
           else
           {
           throw std::runtime_error(  
           "Can not get command interface configuration until the controller is configured." );
           }
          }
          
          controller_interface::InterfaceConfiguration
          TestChainableController::state_interface_configuration(   ) const
          {
           if (  
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           return state_iface_cfg_;
           }
           else
           {
           throw std::runtime_error(  
           "Can not get state interface configuration until the controller is configured." );
           }
          }
          
          controller_interface::return_type TestChainableController::update_reference_from_subscribers(   )
          {
           for (  size_t i = 0; i < reference_interfaces_.size(   ); ++i )
           {
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),  
           "Value of reference interface '%s' before checking external input is %f",  
           (  std::string(  get_node(   )->get_name(   ) ) + "/" + reference_interface_names_[i] ).c_str(   ),  
           reference_interfaces_[i] );
           }
          
           auto joint_commands = rt_command_ptr_.readFromRT(   );
           reference_interfaces_ = (  *joint_commands )->data;
           for (  size_t i = 0; i < reference_interfaces_.size(   ); ++i )
           {
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),  
           "Updated value of reference interface '%s' after applying external input is %f",  
           (  std::string(  get_node(   )->get_name(   ) ) + "/" + reference_interface_names_[i] ).c_str(   ),  
           reference_interfaces_[i] );
           }
          
           return controller_interface::return_type::OK;
          }
          
          controller_interface::return_type TestChainableController::update_and_write_commands(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           ++internal_counter;
          
           for (  size_t i = 0; i < command_interfaces_.size(   ); ++i )
           {
           command_interfaces_[i].set_value(  reference_interfaces_[i] - state_interfaces_[i].get_value(   ) );
           }
          
           return controller_interface::return_type::OK;
          }
          
          CallbackReturn TestChainableController::on_init(   ) { return CallbackReturn::SUCCESS; }
          
          CallbackReturn TestChainableController::on_configure(  
           const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           joints_command_subscriber_ = get_node(   )->create_subscription<CmdType>(  
           "~/commands",   rclcpp::SystemDefaultsQoS(   ),  
           [this](  const CmdType::SharedPtr msg )
           {
           auto joint_commands = rt_command_ptr_.readFromNonRT(   );
          
           if (  msg->data.size(   ) != (  *joint_commands )->data.size(   ) )
           {
           rt_command_ptr_.writeFromNonRT(  msg );
           }
           else
           {
           RCLCPP_ERROR_THROTTLE(  
           get_node(   )->get_logger(   ),   *get_node(   )->get_clock(   ),   1000,  
           "command size (  %zu ) does not match number of reference interfaces (  %zu )",  
           (  *joint_commands )->data.size(   ),   reference_interfaces_.size(   ) );
           }
           } );
          
           auto msg = std::make_shared<CmdType>(   );
           msg->data.resize(  reference_interfaces_.size(   ) );
           rt_command_ptr_.writeFromNonRT(  msg );
          
           return CallbackReturn::SUCCESS;
          }
          
          CallbackReturn TestChainableController::on_activate(  
           const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           if (  !is_in_chained_mode(   ) )
           {
           auto msg = rt_command_ptr_.readFromRT(   );
           (  *msg )->data = reference_interfaces_;
           }
          
           return CallbackReturn::SUCCESS;
          }
          
          CallbackReturn TestChainableController::on_cleanup(  
           const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           joints_command_subscriber_.reset(   );
           return CallbackReturn::SUCCESS;
          }
          
          std::vector<hardware_interface::CommandInterface>
          TestChainableController::on_export_reference_interfaces(   )
          {
           std::vector<hardware_interface::CommandInterface> reference_interfaces;
          
           for (  size_t i = 0; i < reference_interface_names_.size(   ); ++i )
           {
           reference_interfaces.push_back(  hardware_interface::CommandInterface(  
           get_node(   )->get_name(   ),   reference_interface_names_[i],   &reference_interfaces_[i] ) );
           }
          
           return reference_interfaces;
          }
          
          void TestChainableController::set_command_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg )
          {
           cmd_iface_cfg_ = cfg;
          }
          
          void TestChainableController::set_state_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg )
          {
           state_iface_cfg_ = cfg;
          }
          
          void TestChainableController::set_reference_interface_names(  
           const std::vector<std::string> & reference_interface_names )
          {
           reference_interface_names_ = reference_interface_names;
          
           reference_interfaces_.resize(  reference_interface_names.size(   ),   0.0 );
          }
          
          } // namespace test_chainable_controller
          
          #include "pluginlib/class_list_macros.hpp"
          
          PLUGINLIB_EXPORT_CLASS(  
           test_chainable_controller::TestChainableController,  
           controller_interface::ChainableControllerInterface )

ros2_control/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp

       1  // Copyright (  c ) 2022,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_
          #define TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/chainable_controller_interface.hpp"
          #include "controller_manager/visibility_control.h"
          #include "rclcpp/subscription.hpp"
          #include "realtime_tools/realtime_buffer.h"
          #include "std_msgs/msg/float64_multi_array.hpp"
          
          namespace test_chainable_controller
          {
          using CmdType = std_msgs::msg::Float64MultiArray;
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          // indicating the node name under which the controller node
          // is being loaded.
      35  constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name";
          // corresponds to the name listed within the pluginlib xml
      37  constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller";
      38  class TestChainableController : public controller_interface::ChainableControllerInterface
          {
          public:
           CONTROLLER_MANAGER_PUBLIC
           TestChainableController(   );
          
           CONTROLLER_MANAGER_PUBLIC
           virtual ~TestChainableController(   ) = default;
          
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_init(   ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_activate(  const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces(   ) override;
          
           controller_interface::return_type update_reference_from_subscribers(   ) override;
          
           controller_interface::return_type update_and_write_commands(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           // Testing-relevant methods
           CONTROLLER_MANAGER_PUBLIC
           void set_command_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg );
          
           CONTROLLER_MANAGER_PUBLIC
           void set_state_interface_configuration(  const controller_interface::InterfaceConfiguration & cfg );
          
           CONTROLLER_MANAGER_PUBLIC
           void set_reference_interface_names(  const std::vector<std::string> & reference_interface_names );
          
           size_t internal_counter;
           controller_interface::InterfaceConfiguration cmd_iface_cfg_;
           controller_interface::InterfaceConfiguration state_iface_cfg_;
           std::vector<std::string> reference_interface_names_;
          
           realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
           rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
          };
          
          } // namespace test_chainable_controller
          
          #endif // TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_

ros2_control/controller_manager/test/test_controller/test_controller.cpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_controller.hpp"
          
          #include <memory>
          #include <string>
          
          #include "lifecycle_msgs/msg/state.hpp"
          
          namespace test_controller
          {
      24  TestController::TestController(   )
          : controller_interface::ControllerInterface(   ),  
           cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}
          {
          }
          
          controller_interface::InterfaceConfiguration TestController::command_interface_configuration(   ) const
          {
           if (  
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           return cmd_iface_cfg_;
           }
           else
           {
           throw std::runtime_error(  
           "Can not get command interface configuration until the controller is configured." );
           }
          }
          
          controller_interface::InterfaceConfiguration TestController::state_interface_configuration(   ) const
          {
           if (  
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           return state_iface_cfg_;
           }
           else
           {
           throw std::runtime_error(  
           "Can not get state interface configuration until the controller is configured." );
           }
          }
          
          controller_interface::return_type TestController::update(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           ++internal_counter;
          
           for (  size_t i = 0; i < command_interfaces_.size(   ); ++i )
           {
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),   "Setting value of command interface '%s' to %f",  
           command_interfaces_[i].get_name(   ).c_str(   ),   external_commands_for_testing_[i] );
           command_interfaces_[i].set_value(  external_commands_for_testing_[i] );
           }
          
           return controller_interface::return_type::OK;
          }
          
          CallbackReturn TestController::on_init(   ) { return CallbackReturn::SUCCESS; }
          
          CallbackReturn TestController::on_configure(  const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           return CallbackReturn::SUCCESS;
          }
          
          CallbackReturn TestController::on_cleanup(  const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           if (  simulate_cleanup_failure )
           {
           return CallbackReturn::FAILURE;
           }
          
           if (  cleanup_calls )
           {
           (  *cleanup_calls )++;
           }
           return CallbackReturn::SUCCESS;
          }
          
          void TestController::set_command_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg )
          {
           cmd_iface_cfg_ = cfg;
           external_commands_for_testing_.resize(  cmd_iface_cfg_.names.size(   ),   0.0 );
          }
          
          void TestController::set_state_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg )
          {
           state_iface_cfg_ = cfg;
          }
          
          } // namespace test_controller
          
          #include "pluginlib/class_list_macros.hpp"
          
          PLUGINLIB_EXPORT_CLASS(  test_controller::TestController,   controller_interface::ControllerInterface )

ros2_control/controller_manager/test/test_controller/test_controller.hpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_
          #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "controller_manager/visibility_control.h"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          namespace test_controller
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          // indicating the node name under which the controller node
          // is being loaded.
      32  constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name";
          // corresponds to the name listed within the pluginlib xml
      34  constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller";
      35  class TestController : public controller_interface::ControllerInterface
          {
          public:
           CONTROLLER_MANAGER_PUBLIC
           TestController(   );
          
           CONTROLLER_MANAGER_PUBLIC
           virtual ~TestController(   ) = default;
          
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_init(   ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           CallbackReturn on_cleanup(  const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           void set_command_interface_configuration(  
           const controller_interface::InterfaceConfiguration & cfg );
          
           CONTROLLER_MANAGER_PUBLIC
           void set_state_interface_configuration(  const controller_interface::InterfaceConfiguration & cfg );
          
           size_t internal_counter = 0;
           bool simulate_cleanup_failure = false;
           // Variable where we store when cleanup was called,   pointer because the controller
           // is usually destroyed after cleanup
           size_t * cleanup_calls = nullptr;
           controller_interface::InterfaceConfiguration cmd_iface_cfg_;
           controller_interface::InterfaceConfiguration state_iface_cfg_;
          
           std::vector<double> external_commands_for_testing_;
          };
          
          } // namespace test_controller
          
          #endif // TEST_CONTROLLER__TEST_CONTROLLER_HPP_

ros2_control/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

       1  // Copyright 2021 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_controller_failed_init.hpp"
          
          #include <memory>
          #include <string>
          
          #include "lifecycle_msgs/msg/transition.hpp"
          
          namespace test_controller_failed_init
          {
      24  TestControllerFailedInit::TestControllerFailedInit(   ) : controller_interface::ControllerInterface(   )
          {
          }
          
          rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
      29  TestControllerFailedInit::on_init(   )
          {
           return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
          }
          
      34  controller_interface::return_type TestControllerFailedInit::init(  
      35   const std::string & /* controller_name */,   const std::string & /*namespace_*/,  
      36   const rclcpp::NodeOptions & /*node_options*/ )
          {
           return controller_interface::return_type::ERROR;
          }
          
      41  controller_interface::return_type TestControllerFailedInit::update(  
      42   const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           return controller_interface::return_type::OK;
          }
          
          } // namespace test_controller_failed_init
          
          #include "pluginlib/class_list_macros.hpp"
          
      51  PLUGINLIB_EXPORT_CLASS(  
           test_controller_failed_init::TestControllerFailedInit,   controller_interface::ControllerInterface )

ros2_control/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp

       1  // Copyright 2021 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
          #define TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
          
          #include <memory>
          #include <string>
          
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager/visibility_control.h"
          
          namespace test_controller_failed_init
          {
          // indicating the node name under which the controller node
          // is being loaded.
      28  constexpr char TEST_CONTROLLER_NAME[] = "test_controller_failed_init_name";
          // corresponds to the name listed within the pluginlib xml
      30  constexpr char TEST_CONTROLLER_FAILED_INIT_CLASS_NAME[] =
           "controller_manager/test_controller_failed_init";
      32  class TestControllerFailedInit : public controller_interface::ControllerInterface
          {
          public:
           CONTROLLER_MANAGER_PUBLIC
           TestControllerFailedInit(   );
          
           CONTROLLER_MANAGER_PUBLIC
           virtual ~TestControllerFailedInit(   ) = default;
          
           CONTROLLER_INTERFACE_PUBLIC
           controller_interface::return_type init(  
           const std::string & controller_name,   const std::string & namespace_ = "",  
           const rclcpp::NodeOptions & node_options =
           rclcpp::NodeOptions(   )
           .allow_undeclared_parameters(  true )
           .automatically_declare_parameters_from_overrides(  true ) ) override;
          
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(   ) override;
          };
          
          } // namespace test_controller_failed_init
          
          #endif // TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_

ros2_control/controller_manager/test/test_controller_manager.cpp

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_msgs/srv/list_controllers.hpp"
          #include "controller_manager_test_common.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "test_controller/test_controller.hpp"
          
          using ::testing::_;
          using ::testing::Return;
          
      30  class TestControllerManager
      31  : public ControllerManagerFixture<controller_manager::ControllerManager>,  
      32   public testing::WithParamInterface<Strictness>
          {
          };
          
      36  TEST_P(  TestControllerManager,   controller_lifecycle )
          {
           const auto test_param = GetParam(   );
           auto test_controller = std::make_shared<test_controller::TestController>(   );
           auto test_controller2 = std::make_shared<test_controller::TestController>(   );
           constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
           cm_->add_controller(  
           test_controller,   test_controller::TEST_CONTROLLER_NAME,  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           test_controller2,   TEST_CONTROLLER2_NAME,   test_controller::TEST_CONTROLLER_CLASS_NAME );
           EXPECT_EQ(  2u,   cm_->get_loaded_controllers(   ).size(   ) );
           EXPECT_EQ(  2,   test_controller.use_count(   ) );
          
           // setup interface to claim from controllers
           controller_interface::InterfaceConfiguration cmd_itfs_cfg;
           cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           for (  const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES )
           {
           cmd_itfs_cfg.names.push_back(  interface );
           }
           test_controller->set_command_interface_configuration(  cmd_itfs_cfg );
          
           controller_interface::InterfaceConfiguration state_itfs_cfg;
           state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           for (  const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES )
           {
           state_itfs_cfg.names.push_back(  interface );
           }
           for (  const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES )
           {
           state_itfs_cfg.names.push_back(  interface );
           }
           test_controller->set_state_interface_configuration(  state_itfs_cfg );
          
           controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
           cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           for (  const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES )
           {
           cmd_itfs_cfg2.names.push_back(  interface );
           }
           test_controller2->set_command_interface_configuration(  cmd_itfs_cfg2 );
          
           controller_interface::InterfaceConfiguration state_itfs_cfg2;
           state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
           test_controller2->set_state_interface_configuration(  state_itfs_cfg2 );
          
           // Check if namespace is set correctly
           RCLCPP_INFO(  
           rclcpp::get_logger(  "test_controller_manager" ),   "Controller Manager namespace is '%s'",  
           cm_->get_namespace(   ) );
           EXPECT_STREQ(  cm_->get_namespace(   ),   "/" );
           RCLCPP_INFO(  
           rclcpp::get_logger(  "test_controller_manager" ),   "Controller 1 namespace is '%s'",  
           test_controller->get_node(   )->get_namespace(   ) );
           EXPECT_STREQ(  test_controller->get_node(   )->get_namespace(   ),   "/" );
           RCLCPP_INFO(  
           rclcpp::get_logger(  "test_controller_manager" ),   "Controller 2 namespace is '%s'",  
           test_controller2->get_node(   )->get_namespace(   ) );
           EXPECT_STREQ(  test_controller2->get_node(   )->get_namespace(   ),   "/" );
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter )
           << "Update should not reach an unconfigured controller";
          
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   test_controller->get_state(   ).id(   ) );
          
           // configure controller
           cm_->configure_controller(  test_controller::TEST_CONTROLLER_NAME );
           cm_->configure_controller(  TEST_CONTROLLER2_NAME );
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter ) << "Controller is not started";
           EXPECT_EQ(  0u,   test_controller2->internal_counter ) << "Controller is not started";
          
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   test_controller->get_state(   ).id(   ) );
          
           // Start controller,   will take effect at the end of the update function
           std::vector<std::string> start_controllers = {"fake_controller",   TEST_CONTROLLER2_NAME};
           std::vector<std::string> stop_controllers = {};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   test_param.strictness,   true,   rclcpp::Duration(  0,   0 ) );
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller2->internal_counter ) << "Controller is started at the end of update";
           {
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  test_param.expected_return,   switch_future.get(   ) );
           }
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_GE(  test_controller2->internal_counter,   test_param.expected_counter );
          
           // Start the real test controller,   will take effect at the end of the update function
           start_controllers = {test_controller::TEST_CONTROLLER_NAME};
           stop_controllers = {};
           switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   test_param.strictness,   true,   rclcpp::Duration(  0,   0 ) );
          
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter ) << "Controller is started at the end of update";
           {
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           }
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   test_controller->get_state(   ).id(   ) );
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_GE(  test_controller->internal_counter,   1u );
           auto last_internal_counter = test_controller->internal_counter;
          
           // Stop controller,   will take effect at the end of the update function
           start_controllers = {};
           stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
           switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   test_param.strictness,   true,   rclcpp::Duration(  0,   0 ) );
          
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  last_internal_counter + 1u,   test_controller->internal_counter )
           << "Controller is stopped at the end of update,   so it should have done one more update";
           {
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           }
          
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   test_controller->get_state(   ).id(   ) );
           auto unload_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::unload_controller,   cm_,  
           test_controller::TEST_CONTROLLER_NAME );
          
           ASSERT_EQ(  std::future_status::timeout,   unload_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "unload_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   unload_future.get(   ) );
          
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   test_controller->get_state(   ).id(   ) );
           EXPECT_EQ(  1,   test_controller.use_count(   ) );
          }
          
     199  TEST_P(  TestControllerManager,   per_controller_update_rate )
          {
           auto strictness = GetParam(   ).strictness;
           auto test_controller = std::make_shared<test_controller::TestController>(   );
           cm_->add_controller(  
           test_controller,   test_controller::TEST_CONTROLLER_NAME,  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           EXPECT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
           EXPECT_EQ(  2,   test_controller.use_count(   ) );
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter )
           << "Update should not reach an unconfigured controller";
          
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   test_controller->get_state(   ).id(   ) );
          
           test_controller->get_node(   )->set_parameter(  {"update_rate",   4} );
           // configure controller
           cm_->configure_controller(  test_controller::TEST_CONTROLLER_NAME );
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter ) << "Controller is not started";
          
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   test_controller->get_state(   ).id(   ) );
          
           // Start controller,   will take effect at the end of the update function
           std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
           std::vector<std::string> stop_controllers = {};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   strictness,   true,   rclcpp::Duration(  0,   0 ) );
          
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_EQ(  0u,   test_controller->internal_counter ) << "Controller is started at the end of update";
           {
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           }
          
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   test_controller->get_state(   ).id(   ) );
          
           EXPECT_EQ(  
           controller_interface::return_type::OK,  
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ) );
           EXPECT_GE(  test_controller->internal_counter,   1u );
           EXPECT_EQ(  test_controller->get_update_rate(   ),   4u );
          }
          
     256  INSTANTIATE_TEST_SUITE_P(  
           test_strict_best_effort,   TestControllerManager,   testing::Values(  strict,   best_effort ) );

ros2_control/controller_manager/test/test_controller_manager_srvs.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_manager_test_common.hpp"
          
          #include "controller_interface/controller_interface.hpp"
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_msgs/srv/list_controller_types.hpp"
          #include "controller_manager_msgs/srv/list_controllers.hpp"
          #include "controller_manager_msgs/srv/switch_controller.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "test_controller/test_controller.hpp"
          
          using ::testing::_;
          using ::testing::Return;
          using ::testing::UnorderedElementsAre;
          
      35  TEST_F(  TestControllerManagerSrvs,   list_controller_types )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::ListControllerTypes>(  
           "test_controller_manager/list_controller_types" );
           auto request = std::make_shared<controller_manager_msgs::srv::ListControllerTypes::Request>(   );
          
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           // Number depends on the controllers that exist on the system
           size_t controller_types = result->types.size(   );
           ASSERT_GE(  controller_types,   1u );
           ASSERT_EQ(  controller_types,   result->base_classes.size(   ) );
           ASSERT_THAT(  result->types,   ::testing::Contains(  test_controller::TEST_CONTROLLER_CLASS_NAME ) );
           ASSERT_THAT(  
           result->base_classes,   ::testing::Contains(  "controller_interface::ControllerInterface" ) );
           ASSERT_THAT(  
           result->base_classes,  
           ::testing::Contains(  "controller_interface::ChainableControllerInterface" ) );
          }
          
      58  TEST_F(  TestControllerManagerSrvs,   list_controllers_srv )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::ListControllers>(  
           "test_controller_manager/list_controllers" );
           auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>(   );
          
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  0u,   result->controller.size(   ) );
          
           auto test_controller = std::make_shared<test_controller::TestController>(   );
           controller_interface::InterfaceConfiguration cmd_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {"joint1/position",   "joint2/velocity"}};
           controller_interface::InterfaceConfiguration state_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {"joint1/position",   "joint1/velocity",   "joint2/position"}};
           test_controller->set_command_interface_configuration(  cmd_cfg );
           test_controller->set_state_interface_configuration(  state_cfg );
           auto abstract_test_controller = cm_->add_controller(  
           test_controller,   test_controller::TEST_CONTROLLER_NAME,  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           ASSERT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  test_controller::TEST_CONTROLLER_NAME,   result->controller[0].name );
           ASSERT_EQ(  test_controller::TEST_CONTROLLER_CLASS_NAME,   result->controller[0].type );
           ASSERT_EQ(  "unconfigured",   result->controller[0].state );
           ASSERT_TRUE(  result->controller[0].claimed_interfaces.empty(   ) );
           ASSERT_TRUE(  result->controller[0].required_command_interfaces.empty(   ) );
           ASSERT_TRUE(  result->controller[0].required_state_interfaces.empty(   ) );
          
           cm_->configure_controller(  test_controller::TEST_CONTROLLER_NAME );
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  "inactive",   result->controller[0].state );
           ASSERT_TRUE(  result->controller[0].claimed_interfaces.empty(   ) );
           ASSERT_THAT(  
           result->controller[0].required_command_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint2/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_state_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint1/velocity",   "joint2/position" ) );
          
           cm_->switch_controller(  
           {test_controller::TEST_CONTROLLER_NAME},   {},  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,   true,   rclcpp::Duration(  0,   0 ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  "active",   result->controller[0].state );
           ASSERT_THAT(  
           result->controller[0].claimed_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint2/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_command_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint2/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_state_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint1/velocity",   "joint2/position" ) );
          
           cm_->switch_controller(  
           {},   {test_controller::TEST_CONTROLLER_NAME},  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,   true,   rclcpp::Duration(  0,   0 ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  "inactive",   result->controller[0].state );
           ASSERT_TRUE(  result->controller[0].claimed_interfaces.empty(   ) );
           ASSERT_THAT(  
           result->controller[0].required_command_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint2/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_state_interfaces,  
           UnorderedElementsAre(  "joint1/position",   "joint1/velocity",   "joint2/position" ) );
          
           cmd_cfg = {controller_interface::interface_configuration_type::ALL};
           test_controller->set_command_interface_configuration(  cmd_cfg );
           state_cfg = {controller_interface::interface_configuration_type::ALL};
           test_controller->set_state_interface_configuration(  state_cfg );
           cm_->switch_controller(  
           {test_controller::TEST_CONTROLLER_NAME},   {},  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,   true,   rclcpp::Duration(  0,   0 ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  "active",   result->controller[0].state );
           ASSERT_THAT(  
           result->controller[0].claimed_interfaces,  
           UnorderedElementsAre(  
           "joint2/velocity",   "joint3/velocity",   "joint2/max_acceleration",   "configuration/max_tcp_jerk",  
           "joint1/position",   "joint1/max_velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_command_interfaces,  
           UnorderedElementsAre(  
           "configuration/max_tcp_jerk",   "joint1/max_velocity",   "joint1/position",  
           "joint2/max_acceleration",   "joint2/velocity",   "joint3/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_state_interfaces,  
           UnorderedElementsAre(  
           "configuration/max_tcp_jerk",   "joint1/position",   "joint1/some_unlisted_interface",  
           "joint1/velocity",   "joint2/acceleration",   "joint2/position",   "joint2/velocity",  
           "joint3/acceleration",   "joint3/position",   "joint3/velocity",   "sensor1/velocity" ) );
          
           cm_->switch_controller(  
           {},   {test_controller::TEST_CONTROLLER_NAME},  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,   true,   rclcpp::Duration(  0,   0 ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  1u,   result->controller.size(   ) );
           ASSERT_EQ(  "inactive",   result->controller[0].state );
           ASSERT_TRUE(  result->controller[0].claimed_interfaces.empty(   ) );
           ASSERT_THAT(  
           result->controller[0].required_command_interfaces,  
           UnorderedElementsAre(  
           "configuration/max_tcp_jerk",   "joint1/max_velocity",   "joint1/position",  
           "joint2/max_acceleration",   "joint2/velocity",   "joint3/velocity" ) );
           ASSERT_THAT(  
           result->controller[0].required_state_interfaces,  
           UnorderedElementsAre(  
           "configuration/max_tcp_jerk",   "joint1/position",   "joint1/some_unlisted_interface",  
           "joint1/velocity",   "joint2/acceleration",   "joint2/position",   "joint2/velocity",  
           "joint3/acceleration",   "joint3/position",   "joint3/velocity",   "sensor1/velocity" ) );
          
           ASSERT_EQ(  
           controller_interface::return_type::OK,  
           cm_->unload_controller(  test_controller::TEST_CONTROLLER_NAME ) );
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_EQ(  0u,   result->controller.size(   ) );
          }
          
     192  TEST_F(  TestControllerManagerSrvs,   reload_controller_libraries_srv )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::ReloadControllerLibraries>(  
           "test_controller_manager/reload_controller_libraries" );
           auto request =
           std::make_shared<controller_manager_msgs::srv::ReloadControllerLibraries::Request>(   );
          
           // Reload with no controllers running
           request->force_kill = false;
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_TRUE(  result->ok );
          
           // Add a controller,   but unconfigured
           std::shared_ptr<test_controller::TestController> test_controller =
           std::dynamic_pointer_cast<test_controller::TestController>(  cm_->load_controller(  
           test_controller::TEST_CONTROLLER_NAME,   test_controller::TEST_CONTROLLER_CLASS_NAME ) );
          
           // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and
           // can be completely destroyed before reloading the library
           std::weak_ptr<controller_interface::ControllerInterface> test_controller_weak(  test_controller );
          
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   test_controller->get_state(   ).id(   ) );
           ASSERT_GT(  test_controller.use_count(   ),   1 )
           << "Controller manager should have have a copy of this shared ptr";
          
           size_t cleanup_calls = 0;
           test_controller->cleanup_calls = &cleanup_calls;
           test_controller.reset(   ); // destroy our copy of the controller
          
           request->force_kill = false;
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
           // Cleanup is not called from UNCONFIGURED: https://design.ros2.org/articles/node_lifecycle.html
           ASSERT_EQ(  cleanup_calls,   0u );
           ASSERT_EQ(  test_controller.use_count(   ),   0 )
           << "No more references to the controller after reloading.";
           test_controller.reset(   );
          
           // Add a controller,   but inactive
           test_controller = std::dynamic_pointer_cast<test_controller::TestController>(  cm_->load_controller(  
           test_controller::TEST_CONTROLLER_NAME,   test_controller::TEST_CONTROLLER_CLASS_NAME ) );
           test_controller_weak = test_controller;
           cm_->configure_controller(  test_controller::TEST_CONTROLLER_NAME );
          
           ASSERT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   test_controller->get_state(   ).id(   ) );
           ASSERT_GT(  test_controller.use_count(   ),   1 )
           << "Controller manager should have have a copy of this shared ptr";
          
           cleanup_calls = 0;
           test_controller->cleanup_calls = &cleanup_calls;
           test_controller.reset(   ); // destroy our copy of the controller
          
           request->force_kill = false;
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
           ASSERT_EQ(  cleanup_calls,   1u );
           ASSERT_EQ(  test_controller.use_count(   ),   0 )
           << "No more references to the controller after reloading.";
           test_controller.reset(   );
          
           test_controller = std::dynamic_pointer_cast<test_controller::TestController>(  cm_->load_controller(  
           test_controller::TEST_CONTROLLER_NAME,   test_controller::TEST_CONTROLLER_CLASS_NAME ) );
           test_controller_weak = test_controller;
           cm_->configure_controller(  test_controller::TEST_CONTROLLER_NAME );
           // Start Controller
           cm_->switch_controller(  
           {test_controller::TEST_CONTROLLER_NAME},   {},  
           controller_manager_msgs::srv::SwitchController::Request::STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   test_controller->get_state(   ).id(   ) );
          
           // Failed reload due to active controller
           request->force_kill = false;
           result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_FALSE(  result->ok ) << "Cannot reload if controllers are running";
           ASSERT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   test_controller->get_state(   ).id(   ) );
           ASSERT_GT(  test_controller.use_count(   ),   1 )
           << "Controller manager should still have have a copy of "
           "this shared ptr,   no unloading was performed";
          
           cleanup_calls = 0;
           test_controller->cleanup_calls = &cleanup_calls;
           test_controller.reset(   ); // destroy our copy of the controller
          
           // Force stop active controller
           request->force_kill = true;
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
          
           ASSERT_EQ(  test_controller_weak.use_count(   ),   0 )
           << "No more references to the controller after reloading.";
           ASSERT_EQ(  cleanup_calls,   1u )
           << "Controller should have been stopped and cleaned up with force_kill = true";
          }
          
     291  TEST_F(  TestControllerManagerSrvs,   load_controller_srv )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::LoadController>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::LoadController>(  
           "test_controller_manager/load_controller" );
          
           auto request = std::make_shared<controller_manager_msgs::srv::LoadController::Request>(   );
           request->name = test_controller::TEST_CONTROLLER_NAME;
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_FALSE(  result->ok ) << "There's no param specifying the type for " << request->name;
           rclcpp::Parameter controller_type_parameter(  
           std::string(  test_controller::TEST_CONTROLLER_NAME ) + ".type",  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->set_parameter(  controller_type_parameter );
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
           EXPECT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           cm_->get_loaded_controllers(   )[0].c->get_state(   ).id(   ) );
          }
          
     316  TEST_F(  TestControllerManagerSrvs,   unload_controller_srv )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::UnloadController>(  
           "test_controller_manager/unload_controller" );
          
           auto request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>(   );
           request->name = test_controller::TEST_CONTROLLER_NAME;
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_FALSE(  result->ok ) << "Controller not loaded: " << request->name;
          
           auto test_controller = std::make_shared<test_controller::TestController>(   );
           auto abstract_test_controller = cm_->add_controller(  
           test_controller,   test_controller::TEST_CONTROLLER_NAME,  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           EXPECT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
           EXPECT_EQ(  0u,   cm_->get_loaded_controllers(   ).size(   ) );
          }
          
     341  TEST_F(  TestControllerManagerSrvs,   configure_controller_srv )
          {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>(  "srv_client" );
           srv_executor.add_node(  srv_node );
           rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr client =
           srv_node->create_client<controller_manager_msgs::srv::ConfigureController>(  
           "test_controller_manager/configure_controller" );
          
           auto request = std::make_shared<controller_manager_msgs::srv::ConfigureController::Request>(   );
           request->name = test_controller::TEST_CONTROLLER_NAME;
           auto result = call_service_and_wait(  *client,   request,   srv_executor );
           ASSERT_FALSE(  result->ok ) << "Controller not loaded: " << request->name;
          
           auto test_controller = std::make_shared<test_controller::TestController>(   );
           auto abstract_test_controller = cm_->add_controller(  
           test_controller,   test_controller::TEST_CONTROLLER_NAME,  
           test_controller::TEST_CONTROLLER_CLASS_NAME );
           EXPECT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
          
           result = call_service_and_wait(  *client,   request,   srv_executor,   true );
           ASSERT_TRUE(  result->ok );
           EXPECT_EQ(  1u,   cm_->get_loaded_controllers(   ).size(   ) );
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           cm_->get_loaded_controllers(   )[0].c->get_state(   ).id(   ) );
          }

ros2_control/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp

       1  // Copyright 2021 Department of Engineering Cybernetics,   NTNU.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "test_controller_with_interfaces.hpp"
          
          #include <memory>
          #include <string>
          
          #include "lifecycle_msgs/msg/transition.hpp"
          
          namespace test_controller_with_interfaces
          {
      24  TestControllerWithInterfaces::TestControllerWithInterfaces(   )
          : controller_interface::ControllerInterface(   )
          {
          }
          
          rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
      30  TestControllerWithInterfaces::on_init(   )
          {
           return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
          }
          
      35  controller_interface::return_type TestControllerWithInterfaces::update(  
      36   const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           return controller_interface::return_type::OK;
          }
          
          rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
      42  TestControllerWithInterfaces::on_configure(  const rclcpp_lifecycle::State & /*previous_state&*/ )
          {
           return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
          }
          
          rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
      48  TestControllerWithInterfaces::on_cleanup(  const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
          }
          
          } // namespace test_controller_with_interfaces
          
          #include "pluginlib/class_list_macros.hpp"
          
      57  PLUGINLIB_EXPORT_CLASS(  
           test_controller_with_interfaces::TestControllerWithInterfaces,  
           controller_interface::ControllerInterface )

ros2_control/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp

       1  // Copyright 2020 Department of Engineering Cybernetics,   NTNU
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
          #define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
          
          #include <memory>
          #include <string>
          
          #include "controller_interface/visibility_control.h"
          #include "controller_manager/controller_manager.hpp"
          
          namespace test_controller_with_interfaces
          {
          // Corresponds to the name listed within the pluginglib xml
      27  constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] =
           "controller_manager/test_controller_with_interfaces";
          // Corresponds to the command interface to claim
      30  constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity";
      31  class TestControllerWithInterfaces : public controller_interface::ControllerInterface
          {
          public:
           CONTROLLER_MANAGER_PUBLIC
           TestControllerWithInterfaces(   );
          
           CONTROLLER_MANAGER_PUBLIC
           virtual ~TestControllerWithInterfaces(   ) = default;
          
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {TEST_CONTROLLER_COMMAND_INTERFACE}};
           }
          
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override
           {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
           }
          
           CONTROLLER_MANAGER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(   ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           CONTROLLER_MANAGER_PUBLIC
           rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(  
           const rclcpp_lifecycle::State & previous_state ) override;
          };
          
          } // namespace test_controller_with_interfaces
          
          #endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_

ros2_control/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

          // Copyright 2022 Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_msgs/srv/list_controllers.hpp"
          #include "controller_manager_test_common.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/parameter.hpp"
          #include "test_chainable_controller/test_chainable_controller.hpp"
          #include "test_controller/test_controller.hpp"
          
          // The tests in this file are implementing example of chained-control for DiffDrive robot example:
          // https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use
          // The controller have the names as stated in figure,   but they are simply forwarding values without
          // functionality that their name would suggest
          
      35  class TestControllerChainingWithControllerManager;
          
      37  class TestableTestChainableController : public test_chainable_controller::TestChainableController
          {
           friend TestControllerChainingWithControllerManager;
          
      41   FRIEND_TEST(  TestControllerChainingWithControllerManager,   test_chained_controllers );
      42   FRIEND_TEST(  
      43   TestControllerChainingWithControllerManager,  
      44   test_chained_controllers_auto_switch_to_chained_mode );
          };
          
      47  class TestableControllerManager : public controller_manager::ControllerManager
          {
           friend TestControllerChainingWithControllerManager;
          
      51   FRIEND_TEST(  
      52   TestControllerChainingWithControllerManagerAndChainedControllersParameter,  
      53   test_cm_reading_chained_controllers_parameter );
      54   FRIEND_TEST(  
      55   TestControllerChainingWithControllerManagerAndChainedControllersParameter,  
      56   test_cm_reading_chained_controllers_parameter_failure_group0 );
      57   FRIEND_TEST(  
      58   TestControllerChainingWithControllerManagerAndChainedControllersParameter,  
      59   test_cm_reading_chained_controllers_parameter_failure_wrong_type );
      60   FRIEND_TEST(  
      61   TestControllerChainingWithControllerManagerAndChainedControllersParameter,  
      62   test_cm_reading_chained_controllers_parameter_failure_duplicated_controller );
          
      64   FRIEND_TEST(  TestControllerChainingWithControllerManager,   test_chained_controllers );
      65   FRIEND_TEST(  
      66   TestControllerChainingWithControllerManager,  
      67   test_chained_controllers_auto_switch_to_chained_mode );
          
          public:
      70   TestableControllerManager(  
      71   std::unique_ptr<hardware_interface::ResourceManager> resource_manager,  
      72   std::shared_ptr<rclcpp::Executor> executor,  
      73   const std::string & manager_node_name = "controller_manager",  
      74   const std::string & namespace_ = "" )
           : controller_manager::ControllerManager(  
           std::move(  resource_manager ),   executor,   manager_node_name,   namespace_ )
           {
           }
          };
          
      81  class TestControllerChainingWithControllerManager
      82  : public ControllerManagerFixture<TestableControllerManager>,  
      83   public testing::WithParamInterface<Strictness>
          {
          public:
      86   void SetUp(   )
           {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           cm_ = std::make_shared<TestableControllerManager>(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::diffbot_urdf,   true,   true ),  
           executor_,   TEST_CM_NAME );
           run_updater_ = false;
           }
          
      96   void SetupControllers(   )
           {
           test_param = GetParam(   );
          
           pid_left_wheel_controller = std::make_shared<TestableTestChainableController>(   );
           pid_right_wheel_controller = std::make_shared<TestableTestChainableController>(   );
           diff_drive_controller = std::make_shared<TestableTestChainableController>(   );
           position_tracking_controller = std::make_shared<test_controller::TestController>(   );
          
           // configure Left Wheel controller
           controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,   {"wheel_left/velocity"}};
           controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,   {"wheel_left/velocity"}};
           pid_left_wheel_controller->set_command_interface_configuration(  pid_left_cmd_ifs_cfg );
           pid_left_wheel_controller->set_state_interface_configuration(  pid_left_state_ifs_cfg );
           pid_left_wheel_controller->set_reference_interface_names(  {"velocity"} );
          
           // configure Left Wheel controller
           controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,   {"wheel_right/velocity"}};
           controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,   {"wheel_right/velocity"}};
           pid_right_wheel_controller->set_command_interface_configuration(  pid_right_cmd_ifs_cfg );
           pid_right_wheel_controller->set_state_interface_configuration(  pid_right_state_ifs_cfg );
           pid_right_wheel_controller->set_reference_interface_names(  {"velocity"} );
          
           // configure Diff Drive controller
           controller_interface::InterfaceConfiguration diff_drive_cmd_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {std::string(  PID_LEFT_WHEEL ) + "/velocity",   std::string(  PID_RIGHT_WHEEL ) + "/velocity"}};
           controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {"wheel_left/velocity",   "wheel_right/velocity"}};
           diff_drive_controller->set_command_interface_configuration(  diff_drive_cmd_ifs_cfg );
           diff_drive_controller->set_state_interface_configuration(  diff_drive_state_ifs_cfg );
           diff_drive_controller->set_reference_interface_names(  {"vel_x",   "vel_y",   "rot_z"} );
          
           // configure Position Tracking controller
           controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {std::string(  DIFF_DRIVE_CONTROLLER ) + "/vel_x",  
           std::string(  DIFF_DRIVE_CONTROLLER ) + "/vel_y"}};
           // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel"
           position_tracking_controller->set_command_interface_configuration(  
           position_tracking_cmd_ifs_cfg );
           }
          
     144   void CheckIfControllersAreAddedCorrectly(   )
           {
           EXPECT_EQ(  4u,   cm_->get_loaded_controllers(   ).size(   ) );
           EXPECT_EQ(  2,   pid_left_wheel_controller.use_count(   ) );
           EXPECT_EQ(  2,   pid_right_wheel_controller.use_count(   ) );
           EXPECT_EQ(  2,   diff_drive_controller.use_count(   ) );
           EXPECT_EQ(  2,   position_tracking_controller.use_count(   ) );
          
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           pid_left_wheel_controller->get_state(   ).id(   ) );
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           pid_right_wheel_controller->get_state(   ).id(   ) );
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           diff_drive_controller->get_state(   ).id(   ) );
           EXPECT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           position_tracking_controller->get_state(   ).id(   ) );
           }
          
           // order or controller configuration is not important therefore we can reuse the same method
     167   void ConfigureAndCheckControllers(   )
           {
           // Store initial values of command interfaces
           size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys(   ).size(   );
          
           // configure chainable controller and check exported interfaces
           cm_->configure_controller(  PID_LEFT_WHEEL );
           EXPECT_EQ(  
           pid_left_wheel_controller->get_state(   ).id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  cm_->resource_manager_->command_interface_keys(   ).size(   ),   number_of_cmd_itfs + 1 );
           for (  const auto & interface : {"pid_left_wheel_controller/velocity"} )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
          
           cm_->configure_controller(  PID_RIGHT_WHEEL );
           EXPECT_EQ(  
           pid_right_wheel_controller->get_state(   ).id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  cm_->resource_manager_->command_interface_keys(   ).size(   ),   number_of_cmd_itfs + 2 );
           for (  const auto & interface : {"pid_right_wheel_controller/velocity"} )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
          
           cm_->configure_controller(  DIFF_DRIVE_CONTROLLER );
           EXPECT_EQ(  
           diff_drive_controller->get_state(   ).id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  cm_->resource_manager_->command_interface_keys(   ).size(   ),   number_of_cmd_itfs + 5 );
           for (  const auto & interface :
           {"diff_drive_controller/vel_x",   "diff_drive_controller/vel_y",  
           "diff_drive_controller/rot_z"} )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
          
           cm_->configure_controller(  POSITION_TRACKING_CONTROLLER );
           EXPECT_EQ(  
           position_tracking_controller->get_state(   ).id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  cm_->resource_manager_->command_interface_keys(   ).size(   ),   number_of_cmd_itfs + 5 );
           }
          
           template <
           typename T,   typename std::enable_if<
           std::is_convertible<T *,   controller_interface::ControllerInterfaceBase *>::value,  
           T>::type * = nullptr>
           void SetToChainedModeAndMakeReferenceInterfacesAvailable(  
           std::shared_ptr<T> & controller,   const std::string & controller_name,  
           const std::vector<std::string> & reference_interfaces )
           {
           controller->set_chained_mode(  true );
           EXPECT_TRUE(  controller->is_in_chained_mode(   ) );
           // make reference interface command_interfaces available
           cm_->resource_manager_->make_controller_reference_interfaces_available(  controller_name );
           for (  const auto & interface : reference_interfaces )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
           }
          
           template <
           typename T,   typename std::enable_if<
           std::is_convertible<T *,   controller_interface::ControllerInterfaceBase *>::value,  
           T>::type * = nullptr>
           void check_after_de_activate(  
           std::shared_ptr<T> & controller,   const std::vector<std::string> & claimed_command_itfs,  
           size_t expected_internal_counter,   const controller_interface::return_type expected_return,  
           bool deactivated,   bool claimed_interfaces_from_hw = false )
           {
           for (  const auto & interface : claimed_command_itfs )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           // successful xor deactivated
           if (  (  expected_return == controller_interface::return_type::OK ) != deactivated )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
           else
           {
           if (  claimed_interfaces_from_hw )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           }
           else
           {
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           }
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
           }
           if (  expected_internal_counter != 0 )
           {
           ASSERT_EQ(  controller->internal_counter,   expected_internal_counter );
           }
           }
          
           template <
           typename T,   typename std::enable_if<
           std::is_convertible<T *,   controller_interface::ControllerInterfaceBase *>::value,  
           T>::type * = nullptr>
           void ActivateAndCheckController(  
           std::shared_ptr<T> & controller,   const std::string & controller_name,  
           const std::vector<std::string> & claimed_command_itfs,   size_t expected_internal_counter = 0u,  
           const controller_interface::return_type expected_return = controller_interface::return_type::OK )
           {
           switch_test_controllers(  
           {controller_name},   {},   test_param.strictness,   std::future_status::timeout,   expected_return );
           check_after_de_activate(  
           controller,   claimed_command_itfs,   expected_internal_counter,   expected_return,   false );
           }
          
           template <
           typename T,   typename std::enable_if<
           std::is_convertible<T *,   controller_interface::ControllerInterfaceBase *>::value,  
           T>::type * = nullptr>
           void DeactivateAndCheckController(  
           std::shared_ptr<T> & controller,   const std::string & controller_name,  
           const std::vector<std::string> & claimed_command_itfs,   size_t expected_internal_counter = 0u,  
           const bool claimed_interfaces_from_hw = false,  
           const controller_interface::return_type expected_return = controller_interface::return_type::OK )
           {
           switch_test_controllers(  
           {},   {controller_name},   test_param.strictness,   std::future_status::timeout,   expected_return );
           check_after_de_activate(  
           controller,   claimed_command_itfs,   expected_internal_counter,   expected_return,   true,  
           claimed_interfaces_from_hw );
           }
          
           void UpdateAllControllerAndCheck(  
           const std::vector<double> & reference,   size_t exp_internal_counter_pos_ctrl )
           {
           // test value that could cause bad-memory access --> cleaner error during writing tests
           ASSERT_EQ(  reference.size(   ),   2u );
          
           position_tracking_controller->external_commands_for_testing_[0] = reference[0];
           position_tracking_controller->external_commands_for_testing_[1] = reference[1];
          
           cm_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           cm_->resource_manager_->read(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           // check if all controllers are updated
           ASSERT_EQ(  position_tracking_controller->internal_counter,   exp_internal_counter_pos_ctrl );
           ASSERT_EQ(  diff_drive_controller->internal_counter,   exp_internal_counter_pos_ctrl + 2 );
           ASSERT_EQ(  pid_left_wheel_controller->internal_counter,   exp_internal_counter_pos_ctrl + 6 );
           ASSERT_EQ(  pid_right_wheel_controller->internal_counter,   exp_internal_counter_pos_ctrl + 4 );
          
           // check if values are set properly in controllers
           ASSERT_EQ(  
           diff_drive_controller->reference_interfaces_[0],   reference[0] ); // command from Position to
           ASSERT_EQ(  
           diff_drive_controller->reference_interfaces_[1],   reference[1] ); // DiffDrive is forwarded
          
           // Command of DiffDrive controller are references of PID controllers
           EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(  reference[0],   EXP_LEFT_WHEEL_HW_STATE );
           EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(  reference[1],   EXP_RIGHT_WHEEL_HW_STATE );
           ASSERT_EQ(  diff_drive_controller->command_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_REF );
           ASSERT_EQ(  diff_drive_controller->command_interfaces_[1].get_value(   ),   EXP_RIGHT_WHEEL_REF );
           ASSERT_EQ(  pid_left_wheel_controller->reference_interfaces_[0],   EXP_LEFT_WHEEL_REF );
           ASSERT_EQ(  pid_right_wheel_controller->reference_interfaces_[0],   EXP_RIGHT_WHEEL_REF );
          
           EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(  EXP_LEFT_WHEEL_REF,   EXP_LEFT_WHEEL_HW_STATE );
           EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(  EXP_LEFT_WHEEL_CMD );
           ASSERT_EQ(  pid_left_wheel_controller->command_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_CMD );
           ASSERT_EQ(  pid_left_wheel_controller->state_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_HW_STATE );
           // DiffDrive uses the same state
           ASSERT_EQ(  diff_drive_controller->state_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_HW_STATE );
          
           EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(  EXP_RIGHT_WHEEL_REF,   EXP_RIGHT_WHEEL_HW_STATE );
           EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(  EXP_RIGHT_WHEEL_CMD );
           ASSERT_EQ(  pid_right_wheel_controller->command_interfaces_[0].get_value(   ),   EXP_RIGHT_WHEEL_CMD );
           ASSERT_EQ(  
           pid_right_wheel_controller->state_interfaces_[0].get_value(   ),   EXP_RIGHT_WHEEL_HW_STATE );
           // DiffDrive uses the same state
           ASSERT_EQ(  diff_drive_controller->state_interfaces_[1].get_value(   ),   EXP_RIGHT_WHEEL_HW_STATE );
           }
          
           // check data propagation through controllers and if individual controllers are working
           double chained_ctrl_calculation(  double reference,   double state ) { return reference - state; }
           double hardware_calculation(  double command ) { return command / 2.0; }
          
           // set controllers' names
           static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller";
           static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller";
           static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller";
           static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller";
          
           const std::vector<std::string> PID_LEFT_WHEEL_REFERENCE_INTERFACES = {
           "pid_left_wheel_controller/velocity"};
           const std::vector<std::string> PID_RIGHT_WHEEL_REFERENCE_INTERFACES = {
           "pid_right_wheel_controller/velocity"};
           const std::vector<std::string> DIFF_DRIVE_REFERENCE_INTERFACES = {
           "diff_drive_controller/vel_x",   "diff_drive_controller/vel_y",   "diff_drive_controller/rot_z"};
          
           const std::vector<std::string> PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"};
           const std::vector<std::string> PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"};
           const std::vector<std::string> DIFF_DRIVE_CLAIMED_INTERFACES = {
           "pid_left_wheel_controller/velocity",   "pid_right_wheel_controller/velocity"};
           const std::vector<std::string> POSITION_CONTROLLER_CLAIMED_INTERFACES = {
           "diff_drive_controller/vel_x",   "diff_drive_controller/vel_y"};
          
           // controllers objects
           std::shared_ptr<TestableTestChainableController> pid_left_wheel_controller;
           std::shared_ptr<TestableTestChainableController> pid_right_wheel_controller;
           std::shared_ptr<TestableTestChainableController> diff_drive_controller;
           std::shared_ptr<test_controller::TestController> position_tracking_controller;
          
           testing::WithParamInterface<Strictness>::ParamType test_param;
          
           // expected values for tests - shared between multiple test runs
           double EXP_LEFT_WHEEL_CMD = 0.0;
           double EXP_LEFT_WHEEL_HW_STATE = 0.0;
           double EXP_RIGHT_WHEEL_CMD = 0.0;
           double EXP_RIGHT_WHEEL_HW_STATE = 0.0;
           double EXP_LEFT_WHEEL_REF = 0.0;
           double EXP_RIGHT_WHEEL_REF = 0.0;
          };
          
          // The tests are implementing example of chained-control for DiffDrive robot shown here:
          // https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use
          // The controller have the names as stated in figure,   but they are simply forwarding values without
          // functionality that their name would suggest
          TEST_P(  TestControllerChainingWithControllerManager,   test_chained_controllers )
          {
           SetupControllers(   );
          
           // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER
           cm_->add_controller(  
           position_tracking_controller,   POSITION_TRACKING_CONTROLLER,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
          
           CheckIfControllersAreAddedCorrectly(   );
          
           ConfigureAndCheckControllers(   );
          
           SetToChainedModeAndMakeReferenceInterfacesAvailable(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,   PID_LEFT_WHEEL_REFERENCE_INTERFACES );
           SetToChainedModeAndMakeReferenceInterfacesAvailable(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,   PID_RIGHT_WHEEL_REFERENCE_INTERFACES );
           SetToChainedModeAndMakeReferenceInterfacesAvailable(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,   DIFF_DRIVE_REFERENCE_INTERFACES );
          
           EXPECT_THROW(  
           cm_->resource_manager_->make_controller_reference_interfaces_available(  
           POSITION_TRACKING_CONTROLLER ),  
           std::out_of_range );
          
           // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
           cm_->get_logger(   ).set_level(  rclcpp::Logger::Level::Debug );
          
           // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER
           // (  otherwise,   interface will be missing )
           ActivateAndCheckController(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,   PID_LEFT_WHEEL_CLAIMED_INTERFACES,   1u );
           ActivateAndCheckController(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,   PID_RIGHT_WHEEL_CLAIMED_INTERFACES,   1u );
           ASSERT_EQ(  pid_left_wheel_controller->internal_counter,   3u );
          
           // Diff-Drive Controller claims the reference interfaces of PID controllers
           ActivateAndCheckController(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,   DIFF_DRIVE_CLAIMED_INTERFACES,   1u );
           ASSERT_EQ(  pid_right_wheel_controller->internal_counter,   3u );
           ASSERT_EQ(  pid_left_wheel_controller->internal_counter,   5u );
          
           // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller
           ActivateAndCheckController(  
           position_tracking_controller,   POSITION_TRACKING_CONTROLLER,  
           POSITION_CONTROLLER_CLAIMED_INTERFACES,   1u );
           // 'rot_z' reference interface is not claimed
           for (  const auto & interface : {"diff_drive_controller/rot_z"} )
           {
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_exists(  interface ) );
           EXPECT_TRUE(  cm_->resource_manager_->command_interface_is_available(  interface ) );
           EXPECT_FALSE(  cm_->resource_manager_->command_interface_is_claimed(  interface ) );
           }
           ASSERT_EQ(  diff_drive_controller->internal_counter,   3u );
           ASSERT_EQ(  pid_right_wheel_controller->internal_counter,   5u );
           ASSERT_EQ(  pid_left_wheel_controller->internal_counter,   7u );
          
           // update controllers
           std::vector<double> reference = {32.0,   128.0};
          
           // update 'Position Tracking' controller
           for (  auto & value : diff_drive_controller->reference_interfaces_ )
           {
           ASSERT_EQ(  value,   0.0 ); // default reference values are 0.0
           }
           position_tracking_controller->external_commands_for_testing_[0] = reference[0];
           position_tracking_controller->external_commands_for_testing_[1] = reference[1];
           position_tracking_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           ASSERT_EQ(  position_tracking_controller->internal_counter,   2u );
          
           ASSERT_EQ(  diff_drive_controller->reference_interfaces_[0],   reference[0] ); // position_controller
           ASSERT_EQ(  diff_drive_controller->reference_interfaces_[1],   reference[1] ); // is pass-through
          
           // update 'Diff Drive' Controller
           diff_drive_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           ASSERT_EQ(  diff_drive_controller->internal_counter,   4u );
           // default reference values are 0.0 - they should be changed now
           EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(  reference[0],   EXP_LEFT_WHEEL_HW_STATE ); // 32-0
           EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(  reference[1],   EXP_RIGHT_WHEEL_HW_STATE ); // 128-0
           ASSERT_EQ(  pid_left_wheel_controller->reference_interfaces_[0],   EXP_LEFT_WHEEL_REF );
           ASSERT_EQ(  pid_right_wheel_controller->reference_interfaces_[0],   EXP_RIGHT_WHEEL_REF );
          
           // update PID controllers that are writing to hardware
           pid_left_wheel_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           ASSERT_EQ(  pid_left_wheel_controller->internal_counter,   8u );
           pid_right_wheel_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           ASSERT_EQ(  pid_right_wheel_controller->internal_counter,   6u );
          
           // update hardware (  'read' is sufficient for test hardware )
           cm_->resource_manager_->read(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // 32 - 0
           EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(  EXP_LEFT_WHEEL_REF,   EXP_LEFT_WHEEL_HW_STATE );
           // 32 / 2
           EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(  EXP_LEFT_WHEEL_CMD );
           ASSERT_EQ(  pid_left_wheel_controller->command_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_CMD );
           ASSERT_EQ(  pid_left_wheel_controller->state_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_HW_STATE );
           // DiffDrive uses the same state
           ASSERT_EQ(  diff_drive_controller->state_interfaces_[0].get_value(   ),   EXP_LEFT_WHEEL_HW_STATE );
          
           // 128 - 0
           EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(  EXP_RIGHT_WHEEL_REF,   EXP_RIGHT_WHEEL_HW_STATE );
           // 128 / 2
           EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(  EXP_RIGHT_WHEEL_CMD );
           ASSERT_EQ(  pid_right_wheel_controller->command_interfaces_[0].get_value(   ),   EXP_RIGHT_WHEEL_CMD );
           ASSERT_EQ(  pid_right_wheel_controller->state_interfaces_[0].get_value(   ),   EXP_RIGHT_WHEEL_HW_STATE );
           // DiffDrive uses the same state
           ASSERT_EQ(  diff_drive_controller->state_interfaces_[1].get_value(   ),   EXP_RIGHT_WHEEL_HW_STATE );
          
           // update all controllers at once and see that all have expected values --> also checks the order
           // of controller execution
          
           reference = {1024.0,   4096.0};
           UpdateAllControllerAndCheck(  reference,   3u );
          
           // TODO(  destogl ): Add here also slow disabling of controllers
          
           // TODO(  destogl ): Activate test parameter use
          }
          
          TEST_P(  
           TestControllerChainingWithControllerManager,   test_chained_controllers_auto_switch_to_chained_mode )
          {
           SetupControllers(   );
          
           // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER
           cm_->add_controller(  
           position_tracking_controller,   POSITION_TRACKING_CONTROLLER,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
           cm_->add_controller(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,  
           test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
          
           CheckIfControllersAreAddedCorrectly(   );
          
           ConfigureAndCheckControllers(   );
          
           // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
           cm_->get_logger(   ).set_level(  rclcpp::Logger::Level::Debug );
           rclcpp::get_logger(  "ControllerManager::utils" ).set_level(  rclcpp::Logger::Level::Debug );
          
           // at beginning controllers are not in chained mode
           EXPECT_FALSE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_FALSE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           // still not in chained mode because no preceding controller is activated
           ActivateAndCheckController(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,   PID_LEFT_WHEEL_CLAIMED_INTERFACES,   1u );
           ActivateAndCheckController(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,   PID_RIGHT_WHEEL_CLAIMED_INTERFACES,   1u );
           EXPECT_FALSE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_FALSE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           // DiffDrive (  preceding ) controller is activated --> PID controller in chained mode
           ActivateAndCheckController(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,   DIFF_DRIVE_CLAIMED_INTERFACES,   1u );
           EXPECT_TRUE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_TRUE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           // PositionController is activated --> all following controller in chained mode
           ActivateAndCheckController(  
           position_tracking_controller,   POSITION_TRACKING_CONTROLLER,  
           POSITION_CONTROLLER_CLAIMED_INTERFACES,   1u );
           EXPECT_TRUE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_TRUE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_TRUE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           UpdateAllControllerAndCheck(  {32.0,   128.0},   2u );
           UpdateAllControllerAndCheck(  {1024.0,   4096.0},   3u );
          
           // Test switch 'from chained mode' when controllers are deactivated
          
           // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore
           DeactivateAndCheckController(  
           position_tracking_controller,   POSITION_TRACKING_CONTROLLER,  
           POSITION_CONTROLLER_CLAIMED_INTERFACES,   4u );
           EXPECT_TRUE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_TRUE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           // DiffDrive (  preceding ) controller is activated --> PID controller in chained mode
           DeactivateAndCheckController(  
           diff_drive_controller,   DIFF_DRIVE_CONTROLLER,   DIFF_DRIVE_CLAIMED_INTERFACES,   8u );
           EXPECT_FALSE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_FALSE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          
           // all controllers are deactivated --> chained mode is not changed
           DeactivateAndCheckController(  
           pid_left_wheel_controller,   PID_LEFT_WHEEL,   PID_LEFT_WHEEL_CLAIMED_INTERFACES,   14u,   true );
           DeactivateAndCheckController(  
           pid_right_wheel_controller,   PID_RIGHT_WHEEL,   PID_RIGHT_WHEEL_CLAIMED_INTERFACES,   14u,   true );
           EXPECT_FALSE(  pid_left_wheel_controller->is_in_chained_mode(   ) );
           EXPECT_FALSE(  pid_right_wheel_controller->is_in_chained_mode(   ) );
           ASSERT_FALSE(  diff_drive_controller->is_in_chained_mode(   ) );
          }
          
          // TODO(  destogl ): Add test case with controllers added in "random" order
          
          // TODO(  destogl ): Think about strictness and chaining controllers
          // new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain
          //
          
          INSTANTIATE_TEST_SUITE_P(  
           test_strict_best_effort,   TestControllerChainingWithControllerManager,  
           testing::Values(  strict,   best_effort ) );

ros2_control/controller_manager/test/test_hardware_management_srvs.cpp

          // Copyright 2022 Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_manager_test_common.hpp"
          
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_msgs/msg/hardware_component_state.hpp"
          #include "controller_manager_msgs/srv/list_controllers.hpp"
          #include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/parameter.hpp"
          
          using ::testing::_;
          using ::testing::Return;
          
          using hardware_interface::lifecycle_state_names::ACTIVE;
          using hardware_interface::lifecycle_state_names::FINALIZED;
          using hardware_interface::lifecycle_state_names::INACTIVE;
          using hardware_interface::lifecycle_state_names::UNCONFIGURED;
          
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE;
          
          using LFC_STATE = lifecycle_msgs::msg::State;
          
          using namespace std::chrono_literals; // NOLINT
          
      59  class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
          {
          public:
           void SetUp(   ) override
           {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           cm_ = std::make_shared<controller_manager::ControllerManager>(  
           std::make_unique<hardware_interface::ResourceManager>(   ),   executor_,   TEST_CM_NAME );
           run_updater_ = false;
          
           cm_->set_parameter(  
           rclcpp::Parameter(  "robot_description",   ros2_control_test_assets::minimal_robot_urdf ) );
           cm_->set_parameter(  rclcpp::Parameter(  
           "activate_components_on_start",   std::vector<std::string>(  {TEST_ACTUATOR_HARDWARE_NAME} ) ) );
      73   cm_->set_parameter(  rclcpp::Parameter(  
           "configure_components_on_start",   std::vector<std::string>(  {TEST_SENSOR_HARDWARE_NAME} ) ) );
          
           std::string robot_description = "";
      77   cm_->get_parameter(  "robot_description",   robot_description );
      78   if (  robot_description.empty(   ) )
           {
           throw std::runtime_error(  
           "Unable to initialize resource manager,   no robot description found." );
           }
          
      84   cm_->init_resource_manager(  robot_description );
          
           SetUpSrvsCMExecutor(   );
           }
          
           void check_component_fileds(  
           const controller_manager_msgs::msg::HardwareComponentState & component,  
           const std::string & name,   const std::string & type,   const std::string & class_type,  
           const uint8_t state_id,   const std::string & state_label )
           {
           EXPECT_EQ(  component.name,   name );
           EXPECT_EQ(  component.type,   type );
           EXPECT_EQ(  component.class_type,   class_type );
           EXPECT_EQ(  component.state.id,   state_id );
           EXPECT_EQ(  component.state.label,   state_label );
           }
          
           void list_hardware_components_and_check(  
           const std::vector<uint8_t> & hw_state_ids,   const std::vector<std::string> & hw_state_labels,  
           const std::vector<std::vector<std::vector<bool>>> & hw_itfs_available_status,  
           const std::vector<std::vector<std::vector<bool>>> & hw_itfs_claimed_status )
           {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr list_srv_node = std::make_shared<rclcpp::Node>(  "list_srv_client" );
           srv_executor.add_node(  list_srv_node );
           rclcpp::Client<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr list_client =
           list_srv_node->create_client<controller_manager_msgs::srv::ListHardwareComponents>(  
           std::string(  TEST_CM_NAME ) + "/list_hardware_components" );
           auto request =
           std::make_shared<controller_manager_msgs::srv::ListHardwareComponents::Request>(   );
          
           auto result = call_service_and_wait(  *list_client,   request,   srv_executor );
          
           auto check_interfaces =
           [](  
           const std::vector<controller_manager_msgs::msg::HardwareInterface> & interfaces,  
           const std::vector<std::string> & interface_names,  
           const std::vector<bool> is_available_status,   const std::vector<bool> is_claimed_status )
           {
           for (  auto i = 0ul; i < interfaces.size(   ); ++i )
           {
           auto it = std::find(  interface_names.begin(   ),   interface_names.end(   ),   interfaces[i].name );
           EXPECT_NE(  it,   interface_names.end(   ) );
           EXPECT_EQ(  interfaces[i].is_available,   is_available_status[i] );
           EXPECT_EQ(  interfaces[i].is_claimed,   is_claimed_status[i] );
           }
           };
          
           for (  const auto & component : result->component )
           {
           if (  component.name == TEST_ACTUATOR_HARDWARE_NAME )
           {
           check_component_fileds(  
           component,   TEST_ACTUATOR_HARDWARE_NAME,   TEST_ACTUATOR_HARDWARE_TYPE,  
           TEST_ACTUATOR_HARDWARE_CLASS_TYPE,   hw_state_ids[0],   hw_state_labels[0] );
           check_interfaces(  
           component.command_interfaces,   TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           hw_itfs_available_status[0][0],   hw_itfs_claimed_status[0][0] );
           check_interfaces(  
           component.state_interfaces,   TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           hw_itfs_available_status[0][1],   hw_itfs_claimed_status[0][1] );
           }
           if (  component.name == TEST_SENSOR_HARDWARE_NAME )
           {
           check_component_fileds(  
           component,   TEST_SENSOR_HARDWARE_NAME,   TEST_SENSOR_HARDWARE_TYPE,  
           TEST_SENSOR_HARDWARE_CLASS_TYPE,   hw_state_ids[1],   hw_state_labels[1] );
           check_interfaces(  
           component.command_interfaces,   TEST_SENSOR_HARDWARE_COMMAND_INTERFACES,  
           hw_itfs_available_status[1][0],   hw_itfs_claimed_status[1][0] );
           check_interfaces(  
           component.state_interfaces,   TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           hw_itfs_available_status[1][1],   hw_itfs_claimed_status[1][1] );
           }
           if (  component.name == TEST_SYSTEM_HARDWARE_NAME )
           {
           check_component_fileds(  
           component,   TEST_SYSTEM_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_TYPE,  
           TEST_SYSTEM_HARDWARE_CLASS_TYPE,   hw_state_ids[2],   hw_state_labels[2] );
           check_interfaces(  
           component.command_interfaces,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           hw_itfs_available_status[2][0],   hw_itfs_claimed_status[2][0] );
           check_interfaces(  
           component.state_interfaces,   TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           hw_itfs_available_status[2][1],   hw_itfs_claimed_status[2][1] );
           }
           }
           }
          
           bool set_hardware_component_state(  
           const std::string & hardware_name,   const uint8_t target_state_id,  
           const std::string & target_state_label )
           {
           rclcpp::executors::SingleThreadedExecutor srv_executor;
           rclcpp::Node::SharedPtr mha_srv_node = std::make_shared<rclcpp::Node>(  "mha_srv_client" );
           srv_executor.add_node(  mha_srv_node );
           rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr mha_client =
           mha_srv_node->create_client<controller_manager_msgs::srv::SetHardwareComponentState>(  
           std::string(  TEST_CM_NAME ) + "/set_hardware_component_state" );
           auto request =
           std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>(   );
          
           request->name = hardware_name;
           request->target_state.id = target_state_id;
           request->target_state.label = target_state_label;
          
           auto result = call_service_and_wait(  *mha_client,   request,   srv_executor );
           return result->ok;
           }
          };
          
          class TestControllerManagerHWManagementSrvsWithoutParams
          : public TestControllerManagerHWManagementSrvs
          {
          public:
           void SetUp(   ) override
           {
           executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
           cm_ = std::make_shared<controller_manager::ControllerManager>(  
           std::make_unique<hardware_interface::ResourceManager>(   ),   executor_,   TEST_CM_NAME );
           run_updater_ = false;
          
           // TODO(  destogl ): separate this to init_tests method where parameter can be set for each test
           // separately
           cm_->set_parameter(  
           rclcpp::Parameter(  "robot_description",   ros2_control_test_assets::minimal_robot_urdf ) );
          
           std::string robot_description = "";
           cm_->get_parameter(  "robot_description",   robot_description );
           if (  robot_description.empty(   ) )
           {
           throw std::runtime_error(  
           "Unable to initialize resource manager,   no robot description found." );
           }
          
           cm_->init_resource_manager(  robot_description );
          
           SetUpSrvsCMExecutor(   );
           }
          };
          
          TEST_F(  TestControllerManagerHWManagementSrvs,   list_hardware_components )
          {
           // Default status after start:
           // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read
          
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_ACTIVE,   LFC_STATE::PRIMARY_STATE_INACTIVE,  
           LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),  
           std::vector<std::string>(  {ACTIVE,   INACTIVE,   UNCONFIGURED} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          }
          
          // TODO(  destogl ): Add tests for testing controller starting/stopping depending on hw state
          TEST_F(  TestControllerManagerHWManagementSrvs,   selective_activate_deactivate_components_set_state )
          {
           using lifecycle_msgs::msg::State;
          
           // Default status after start
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_ACTIVE,   LFC_STATE::PRIMARY_STATE_INACTIVE,  
           LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),  
           std::vector<std::string>(  {ACTIVE,   INACTIVE,   UNCONFIGURED} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          
           // Activate sensor
           set_hardware_component_state(  TEST_SENSOR_HARDWARE_NAME,   State::PRIMARY_STATE_ACTIVE,   "" );
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_ACTIVE,   LFC_STATE::PRIMARY_STATE_ACTIVE,  
           LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),  
           std::vector<std::string>(  {ACTIVE,   ACTIVE,   UNCONFIGURED} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          
           // Activate system directly - it should do configure automatically; ID is determined from label
           set_hardware_component_state(  TEST_SYSTEM_HARDWARE_NAME,   0,   ACTIVE );
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_ACTIVE,   LFC_STATE::PRIMARY_STATE_ACTIVE,  
           LFC_STATE::PRIMARY_STATE_ACTIVE} ),  
           std::vector<std::string>(  {ACTIVE,   ACTIVE,   ACTIVE} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{true,   true,   true,   true},   {true,   true,   true,   true,   true,   true,   true}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          
           // Deactivate actuator
           set_hardware_component_state(  TEST_ACTUATOR_HARDWARE_NAME,   0,   INACTIVE );
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_INACTIVE,   LFC_STATE::PRIMARY_STATE_ACTIVE,  
           LFC_STATE::PRIMARY_STATE_ACTIVE} ),  
           std::vector<std::string>(  {INACTIVE,   ACTIVE,   ACTIVE} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{true,   true,   true,   true},   {true,   true,   true,   true,   true,   true,   true}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          
           // Double activate system
           set_hardware_component_state(  TEST_SYSTEM_HARDWARE_NAME,   LFC_STATE::PRIMARY_STATE_ACTIVE,   ACTIVE );
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_INACTIVE,   LFC_STATE::PRIMARY_STATE_ACTIVE,  
           LFC_STATE::PRIMARY_STATE_ACTIVE} ),  
           std::vector<std::string>(  {INACTIVE,   ACTIVE,   ACTIVE} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{true,   true,   true,   true},   {true,   true,   true,   true,   true,   true,   true}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          
           // Set sensor to UNCONFIGURED - inactive and cleanup transitions has to be automatically done
           set_hardware_component_state(  
           TEST_SENSOR_HARDWARE_NAME,   LFC_STATE::PRIMARY_STATE_UNCONFIGURED,   UNCONFIGURED );
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_INACTIVE,   LFC_STATE::PRIMARY_STATE_UNCONFIGURED,  
           LFC_STATE::PRIMARY_STATE_ACTIVE} ),  
           std::vector<std::string>(  {INACTIVE,   UNCONFIGURED,   ACTIVE} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {false}},   // sensor
           {{true,   true,   true,   true},   {true,   true,   true,   true,   true,   true,   true}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          }
          
          TEST_F(  TestControllerManagerHWManagementSrvsWithoutParams,   test_default_activation_of_all_hardware )
          {
           // "configure_components_on_start" and "activate_components_on_start" are not set (  empty )
           // therefore all hardware components are active
           list_hardware_components_and_check(  
           // actuator,   sensor,   system
           std::vector<uint8_t>(  
           {LFC_STATE::PRIMARY_STATE_ACTIVE,   LFC_STATE::PRIMARY_STATE_ACTIVE,  
           LFC_STATE::PRIMARY_STATE_ACTIVE} ),  
           std::vector<std::string>(  {ACTIVE,   ACTIVE,   ACTIVE} ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is available
           {{true,   true},   {true,   true,   true}},   // actuator
           {{},   {true}},   // sensor
           {{true,   true,   true,   true},   {true,   true,   true,   true,   true,   true,   true}},   // system
           } ),  
           std::vector<std::vector<std::vector<bool>>>(  {
           // is claimed
           {{false,   false},   {false,   false,   false}},   // actuator
           {{},   {false}},   // sensor
           {{false,   false,   false,   false},   {false,   false,   false,   false,   false,   false,   false}},   // system
           } ) );
          }

ros2_control/controller_manager/test/test_release_interfaces.cpp

       1  // Copyright 2021 Department of Engineering Cybernetics,   NTNU.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <gtest/gtest.h>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_test_common.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"
          
          using ::testing::_;
          using ::testing::Return;
          
      30  class TestReleaseInterfaces : public ControllerManagerFixture<controller_manager::ControllerManager>
          {
          };
          
      34  TEST_F(  TestReleaseInterfaces,   switch_controllers_same_interface )
          {
           std::string controller_type =
           test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME;
          
           // Load two controllers of different names
           std::string controller_name1 = "test_controller1";
           std::string controller_name2 = "test_controller2";
           ASSERT_NO_THROW(  cm_->load_controller(  controller_name1,   controller_type ) );
           ASSERT_NO_THROW(  cm_->load_controller(  controller_name2,   controller_type ) );
           ASSERT_EQ(  2u,   cm_->get_loaded_controllers(   ).size(   ) );
           controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers(   )[0];
           controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers(   )[1];
          
           // Configure controllers
           ASSERT_EQ(  controller_interface::return_type::OK,   cm_->configure_controller(  controller_name1 ) );
           ASSERT_EQ(  controller_interface::return_type::OK,   cm_->configure_controller(  controller_name2 ) );
          
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
          
           { // Test starting the first controller
           RCLCPP_INFO(  cm_->get_logger(   ),   "Starting controller #1" );
           std::vector<std::string> start_controllers = {controller_name1};
           std::vector<std::string> stop_controllers = {};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test starting the second controller when the first is running
           // Fails as they have the same command interface
           RCLCPP_INFO(  cm_->get_logger(   ),   "Starting controller #2" );
           std::vector<std::string> start_controllers = {controller_name2};
           std::vector<std::string> stop_controllers = {};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test stopping controller #1 and starting controller #2
           RCLCPP_INFO(  cm_->get_logger(   ),   "Stopping controller #1 and starting controller #2" );
           std::vector<std::string> start_controllers = {controller_name2};
           std::vector<std::string> stop_controllers = {controller_name1};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test stopping controller #2 and starting controller #1
           RCLCPP_INFO(  cm_->get_logger(   ),   "Starting controller #1 and stopping controller #2" );
           std::vector<std::string> start_controllers = {controller_name1};
           std::vector<std::string> stop_controllers = {controller_name2};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test stopping both controllers when only controller #1 is running
           RCLCPP_INFO(  
           cm_->get_logger(   ),  
           "Stopping both controllers (  will fail in STRICT mode as controller #2 is not running )" );
           std::vector<std::string> start_controllers = {};
           std::vector<std::string> stop_controllers = {controller_name1,   controller_name2};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           // The call to switch above will fail and the CM will not block
           // ASSERT_EQ(  
           // std::future_status::timeout,  
           // switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) ) <<
           // "switch_controller should be blocking until next update cycle";
           // ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::ERROR,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test stopping both controllers when only controller #1 is running
           RCLCPP_INFO(  
           cm_->get_logger(   ),  
           "Stopping both controllers (  will fail in BEST EFFORT mode as controller #2 is not running )" );
           std::vector<std::string> start_controllers = {};
           std::vector<std::string> stop_controllers = {controller_name1,   controller_name2};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   BEST_EFFORT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          
           { // Test starting both controllers at the same time
           RCLCPP_INFO(  
           cm_->get_logger(   ),  
           "Starting both controllers at the same time (  should notify about resource conflict )" );
           std::vector<std::string> start_controllers = {controller_name1,   controller_name2};
           std::vector<std::string> stop_controllers = {};
           auto switch_future = std::async(  
           std::launch::async,   &controller_manager::ControllerManager::switch_controller,   cm_,  
           start_controllers,   stop_controllers,   STRICT,   true,   rclcpp::Duration(  0,   0 ) );
           ASSERT_EQ(  std::future_status::timeout,   switch_future.wait_for(  std::chrono::milliseconds(  100 ) ) )
           << "switch_controller should be blocking until next update cycle";
           ControllerManagerRunner cm_runner(  this );
           EXPECT_EQ(  controller_interface::return_type::OK,   switch_future.get(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           abstract_test_controller1.c->get_state(   ).id(   ) );
           ASSERT_EQ(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           abstract_test_controller2.c->get_state(   ).id(   ) );
           }
          }

ros2_control/controller_manager/test/test_spawner_unspawner.cpp

          // Copyright 2021 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          
          #include <cstdlib>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "controller_manager/controller_manager.hpp"
          #include "controller_manager_test_common.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "test_controller/test_controller.hpp"
          
          using ::testing::_;
          using ::testing::Return;
          
          using namespace std::chrono_literals;
      32  class TestLoadController : public ControllerManagerFixture<controller_manager::ControllerManager>
          {
           void SetUp(   ) override
           {
           ControllerManagerFixture::SetUp(   );
          
           update_timer_ = cm_->create_wall_timer(  
           std::chrono::milliseconds(  10 ),  
           [&](   )
           {
           cm_->read(  TIME,   PERIOD );
           cm_->update(  TIME,   PERIOD );
           cm_->write(  TIME,   PERIOD );
           } );
          
      47   update_executor_ =
           std::make_shared<rclcpp::executors::MultiThreadedExecutor>(  rclcpp::ExecutorOptions(   ),   2 );
          
      50   update_executor_->add_node(  cm_ );
           update_executor_spin_future_ =
           std::async(  std::launch::async,   [this](   ) -> void { update_executor_->spin(   ); } );
           // This sleep is needed to prevent a too fast test from ending before the
           // executor has began to spin,   which causes it to hang
           std::this_thread::sleep_for(  50ms );
           }
          
           void TearDown(   ) override { update_executor_->cancel(   ); }
          
          protected:
           rclcpp::TimerBase::SharedPtr update_timer_;
          
           // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
           std::shared_ptr<rclcpp::Executor> update_executor_;
           std::future<void> update_executor_spin_future_;
          };
          
          int call_spawner(  const std::string extra_args )
          {
           std::string spawner_script = "ros2 run controller_manager spawner ";
           return std::system(  (  spawner_script + extra_args ).c_str(   ) );
          }
          
          int call_unspawner(  const std::string extra_args )
          {
           std::string spawner_script = "ros2 run controller_manager unspawner ";
           return std::system(  (  spawner_script + extra_args ).c_str(   ) );
          }
          
          TEST_F(  TestLoadController,   spawner_with_no_arguments_errors )
          {
           EXPECT_NE(  call_spawner(  "" ),   0 ) << "Missing mandatory arguments";
          }
          
          TEST_F(  TestLoadController,   spawner_without_manager_errors )
          {
           EXPECT_NE(  call_spawner(  "ctrl_1" ),   0 ) << "Wrong controller manager name";
          }
          
          TEST_F(  TestLoadController,   spawner_without_type_parameter_or_arg_errors )
          {
           EXPECT_NE(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 ) << "Missing .type parameter";
          }
          
          TEST_F(  TestLoadController,   spawner_test_type_in_param )
          {
           cm_->set_parameter(  rclcpp::Parameter(  "ctrl_1.type",   test_controller::TEST_CONTROLLER_CLASS_NAME ) );
          
           EXPECT_EQ(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 );
          
           ASSERT_EQ(  cm_->get_loaded_controllers(   ).size(   ),   1ul );
           auto ctrl_1 = cm_->get_loaded_controllers(   )[0];
           ASSERT_EQ(  ctrl_1.info.name,   "ctrl_1" );
           ASSERT_EQ(  ctrl_1.info.type,   test_controller::TEST_CONTROLLER_CLASS_NAME );
           ASSERT_EQ(  ctrl_1.c->get_state(   ).id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // Try to spawn again,   it should fail because already active
           EXPECT_NE(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 ) << "Cannot configure from active";
           ctrl_1.c->get_node(   )->deactivate(   );
           // We should be able to reconfigure and activate a configured controller
           EXPECT_EQ(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 );
           ctrl_1.c->get_node(   )->deactivate(   );
           ctrl_1.c->get_node(   )->cleanup(   );
           // We should be able to reconfigure and activate am unconfigured loaded controller
           EXPECT_EQ(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 );
           ASSERT_EQ(  ctrl_1.c->get_state(   ).id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // Unload and reload
           EXPECT_EQ(  call_unspawner(  "ctrl_1 -c test_controller_manager" ),   0 );
           ASSERT_EQ(  cm_->get_loaded_controllers(   ).size(   ),   0ul ) << "Controller should have been unloaded";
           EXPECT_EQ(  call_spawner(  "ctrl_1 -c test_controller_manager" ),   0 );
           ASSERT_EQ(  cm_->get_loaded_controllers(   ).size(   ),   1ul ) << "Controller should have been loaded";
           ctrl_1 = cm_->get_loaded_controllers(   )[0];
           ASSERT_EQ(  ctrl_1.c->get_state(   ).id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          }
          
          TEST_F(  TestLoadController,   spawner_test_type_in_arg )
          {
           // Provide controller type via -t argument
           EXPECT_EQ(  
           call_spawner(  
           "ctrl_2 -c test_controller_manager -t " +
           std::string(  test_controller::TEST_CONTROLLER_CLASS_NAME ) ),  
           0 );
          
           ASSERT_EQ(  cm_->get_loaded_controllers(   ).size(   ),   1ul );
           auto ctrl_2 = cm_->get_loaded_controllers(   )[0];
           ASSERT_EQ(  ctrl_2.info.name,   "ctrl_2" );
           ASSERT_EQ(  ctrl_2.info.type,   test_controller::TEST_CONTROLLER_CLASS_NAME );
           ASSERT_EQ(  ctrl_2.c->get_state(   ).id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          }
          
          TEST_F(  TestLoadController,   unload_on_kill )
          {
           // Launch spawner with unload on kill
           // timeout command will kill it after the specified time with signal SIGINT
           std::stringstream ss;
           ss << "timeout --signal=INT 5 "
           << "ros2 run controller_manager spawner "
           << "ctrl_3 -c test_controller_manager -t "
           << std::string(  test_controller::TEST_CONTROLLER_CLASS_NAME ) << " --unload-on-kill";
          
           EXPECT_NE(  std::system(  ss.str(   ).c_str(   ) ),   0 )
           << "timeout should have killed spawner and returned non 0 code";
          
           ASSERT_EQ(  cm_->get_loaded_controllers(   ).size(   ),   0ul );
          }

ros2_control/hardware_interface/include/fake_components/visibility_control.h

       1  // Copyright (  c ) 2021 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef FAKE_COMPONENTS__VISIBILITY_CONTROL_H_
          #define FAKE_COMPONENTS__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define FAKE_COMPONENTS_EXPORT __attribute__(  (  dllexport ) )
          #define FAKE_COMPONENTS_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define FAKE_COMPONENTS_EXPORT __declspec(  dllexport )
          #define FAKE_COMPONENTS_IMPORT __declspec(  dllimport )
          #endif
          #ifdef FAKE_COMPONENTS_BUILDING_DLL
          #define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_EXPORT
          #else
          #define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_IMPORT
          #endif
          #define FAKE_COMPONENTS_PUBLIC_TYPE FAKE_COMPONENTS_PUBLIC
          #define FAKE_COMPONENTS_LOCAL
          #else
          #define FAKE_COMPONENTS_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define FAKE_COMPONENTS_IMPORT
          #if __GNUC__ >= 4
          #define FAKE_COMPONENTS_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define FAKE_COMPONENTS_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define FAKE_COMPONENTS_PUBLIC
          #define FAKE_COMPONENTS_LOCAL
          #endif
          #define FAKE_COMPONENTS_PUBLIC_TYPE
          #endif
          
          #endif // FAKE_COMPONENTS__VISIBILITY_CONTROL_H_

ros2_control/hardware_interface/include/hardware_interface/actuator.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__ACTUATOR_HPP_
          #define HARDWARE_INTERFACE__ACTUATOR_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/visibility_control.h"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
      32  class ActuatorInterface;
          
          class Actuator final
          {
          public:
      37   Actuator(   ) = default;
          
           HARDWARE_INTERFACE_PUBLIC
           explicit Actuator(  std::unique_ptr<ActuatorInterface> impl );
          
      42   Actuator(  Actuator && other ) = default;
          
      44   ~Actuator(   ) = default;
          
      46   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & initialize(  const HardwareInfo & actuator_info );
          
      49   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & configure(   );
          
           HARDWARE_INTERFACE_PUBLIC
      53   const rclcpp_lifecycle::State & cleanup(   );
          
      55   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & shutdown(   );
          
      58   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & activate(   );
          
           HARDWARE_INTERFACE_PUBLIC
      62   const rclcpp_lifecycle::State & deactivate(   );
          
      64   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & error(   );
          
           HARDWARE_INTERFACE_PUBLIC
           std::vector<StateInterface> export_state_interfaces(   );
          
           HARDWARE_INTERFACE_PUBLIC
           std::vector<CommandInterface> export_command_interfaces(   );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           HARDWARE_INTERFACE_PUBLIC
      84   std::string get_name(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
      87   const rclcpp_lifecycle::State & get_state(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
           return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type write(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
          private:
           std::unique_ptr<ActuatorInterface> impl_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__ACTUATOR_HPP_

ros2_control/hardware_interface/include/hardware_interface/actuator_interface.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
          #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          /// Virtual Class to implement when integrating a 1 DoF actuator into ros2_control.
          /**
           * The typical examples are conveyors or motors.
           *
           * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
           * with the following meaning:
           *
           * \returns CallbackReturn::SUCCESS method execution was successful.
           * \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
           * \returns CallbackReturn::ERROR critical error has happened that should be managed in
           * "on_error" method.
           *
           * The hardware ends after each method in a state with the following meaning:
           *
           * UNCONFIGURED (  on_init,   on_cleanup ):
           * Hardware is initialized but communication is not started and therefore no interface is available.
           *
           * INACTIVE (  on_configure,   on_deactivate ):
           * Communication with the hardware is started and it is configured.
           * States can be read and non-movement hardware interfaces commanded.
           * Hardware interfaces for movement will NOT be available.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           *
           * FINALIZED (  on_shutdown ):
           * Hardware interface is ready for unloading/destruction.
           * Allocated memory is cleaned up.
           *
           * ACTIVE (  on_activate ):
           * Power circuits of hardware are active and hardware can be moved,   e.g.,   brakes are disabled.
           * Command interfaces for movement are available and have to be accepted.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           */
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      69  class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
          {
          public:
      72   ActuatorInterface(   )
           : lifecycle_state_(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN,   lifecycle_state_names::UNKNOWN ) )
           {
           }
          
           /// ActuatorInterface copy constructor is actively deleted.
           /**
           * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
           * failed or simultaneous access to hardware.
           */
           ActuatorInterface(  const ActuatorInterface & other ) = delete;
          
           ActuatorInterface(  ActuatorInterface && other ) = default;
          
           virtual ~ActuatorInterface(   ) = default;
          
           /// Initialization of the hardware interface from data parsed from the robot's URDF.
           /**
           * \param[in] hardware_info structure with data from URDF.
           * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
           * \returns CallbackReturn::ERROR if any error happens or data are missing.
           */
           virtual CallbackReturn on_init(  const HardwareInfo & hardware_info )
           {
           info_ = hardware_info;
           return CallbackReturn::SUCCESS;
           };
          
           /// Exports all state interfaces for this hardware interface.
           /**
           * The state interfaces have to be created and transferred according
           * to the hardware info passed in for the configuration.
           *
           * Note the ownership over the state interfaces is transferred to the caller.
           *
           * \return vector of state interfaces
           */
     110   virtual std::vector<StateInterface> export_state_interfaces(   ) = 0;
          
           /// Exports all command interfaces for this hardware interface.
           /**
           * The command interfaces have to be created and transferred according
           * to the hardware info passed in for the configuration.
           *
           * Note the ownership over the state interfaces is transferred to the caller.
           *
           * \return vector of command interfaces
           */
           virtual std::vector<CommandInterface> export_command_interfaces(   ) = 0;
          
           /// Prepare for a new command interface switch.
           /**
           * Prepare for any mode-switching required by the new command interface combination.
           *
           * \note This is a non-realtime evaluation of whether a set of command interface claims are
           * possible,   and call to start preparing data structures for the upcoming switch that will occur.
           * \note All starting and stopping interface keys are passed to all components,   so the function should
           * return return_type::OK by default when given interface keys not relevant for this component.
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
           * \return return_type::OK if the new command interface combination can be prepared,  
           * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
           */
           virtual return_type prepare_command_mode_switch(  
           const std::vector<std::string> & /*start_interfaces*/,  
           const std::vector<std::string> & /*stop_interfaces*/ )
           {
           return return_type::OK;
           }
          
           // Perform switching to the new command interface.
           /**
           * Perform the mode-switching for the new command interface combination.
           *
           * \note This is part of the realtime update loop,   and should be fast.
           * \note All starting and stopping interface keys are passed to all components,   so the function should
           * return return_type::OK by default when given interface keys not relevant for this component.
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
           * \return return_type::OK if the new command interface combination can be switched to,  
           * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
           */
           virtual return_type perform_command_mode_switch(  
           const std::vector<std::string> & /*start_interfaces*/,  
           const std::vector<std::string> & /*stop_interfaces*/ )
           {
           return return_type::OK;
           }
          
           /// Read the current state values from the actuator.
           /**
           * The data readings from the physical hardware has to be updated
           * and reflected accordingly in the exported state interfaces.
           * That is,   the data pointed by the interfaces shall be updated.
           *
           * \return return_type::OK if the read was successful,   return_type::ERROR otherwise.
           */
           virtual return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Write the current command values to the actuator.
           /**
           * The physical hardware shall be updated with the latest value from
           * the exported command interfaces.
           *
           * \return return_type::OK if the read was successful,   return_type::ERROR otherwise.
           */
           virtual return_type write(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Get name of the actuator hardware.
           /**
           * \return name.
           */
           virtual std::string get_name(   ) const { return info_.name; }
          
           /// Get life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           const rclcpp_lifecycle::State & get_state(   ) const { return lifecycle_state_; }
          
           /// Set life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           void set_state(  const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
          
          protected:
           HardwareInfo info_;
           rclcpp_lifecycle::State lifecycle_state_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_

ros2_control/hardware_interface/include/hardware_interface/component_parser.hpp

       1  // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
          #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
          
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/visibility_control.h"
          
          namespace hardware_interface
          {
          /// Search XML snippet from URDF for information about a control component.
          /**
           * \param[in] urdf string with robot's URDF
           * \return vector filled with information about robot's control resources
           * \throws std::runtime_error if a robot attribute or tag is not found
           */
          HARDWARE_INTERFACE_PUBLIC
          std::vector<HardwareInfo> parse_control_resources_from_urdf(  const std::string & urdf );
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

ros2_control/hardware_interface/include/hardware_interface/controller_info.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
          #define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
          
          #include <string>
          #include <vector>
          
          namespace hardware_interface
          {
          /// Controller Information
          /**
           * This struct contains information about a given controller.
           */
          struct ControllerInfo
          {
           /// Controller name.
           std::string name;
          
           /// Controller type.
           std::string type;
          
           /// List of claimed interfaces by the controller.
           std::vector<std::string> claimed_interfaces;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_

ros2_control/hardware_interface/include/hardware_interface/handle.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
          #define HARDWARE_INTERFACE__HANDLE_HPP_
          
          #include <string>
          #include <utility>
          
          #include "hardware_interface/macros.hpp"
          #include "hardware_interface/visibility_control.h"
          
          namespace hardware_interface
          {
          /// A handle used to get and set a value on a given interface.
      27  class ReadOnlyHandle
          {
          public:
      30   ReadOnlyHandle(  
           const std::string & prefix_name,   const std::string & interface_name,  
           double * value_ptr = nullptr )
           : prefix_name_(  prefix_name ),   interface_name_(  interface_name ),   value_ptr_(  value_ptr )
           {
           }
          
      37   explicit ReadOnlyHandle(  const std::string & interface_name )
           : interface_name_(  interface_name ),   value_ptr_(  nullptr )
           {
           }
          
      42   explicit ReadOnlyHandle(  const char * interface_name )
           : interface_name_(  interface_name ),   value_ptr_(  nullptr )
           {
           }
          
           ReadOnlyHandle(  const ReadOnlyHandle & other ) = default;
          
           ReadOnlyHandle(  ReadOnlyHandle && other ) = default;
          
           ReadOnlyHandle & operator=(  const ReadOnlyHandle & other ) = default;
          
           ReadOnlyHandle & operator=(  ReadOnlyHandle && other ) = default;
          
           virtual ~ReadOnlyHandle(   ) = default;
          
           /// Returns true if handle references a value.
           inline operator bool(   ) const { return value_ptr_ != nullptr; }
          
           const std::string get_name(   ) const { return prefix_name_ + "/" + interface_name_; }
          
      62   const std::string & get_interface_name(   ) const { return interface_name_; }
          
           [[deprecated(  
           "Replaced by get_name method,   which is semantically more correct" )]] const std::string
           get_full_name(   ) const
           {
           return get_name(   );
           }
          
           const std::string & get_prefix_name(   ) const { return prefix_name_; }
          
           double get_value(   ) const
           {
           THROW_ON_NULLPTR(  value_ptr_ );
           return *value_ptr_;
           }
          
          protected:
           std::string prefix_name_;
           std::string interface_name_;
           double * value_ptr_;
          };
          
          class ReadWriteHandle : public ReadOnlyHandle
          {
          public:
           ReadWriteHandle(  
           const std::string & prefix_name,   const std::string & interface_name,  
           double * value_ptr = nullptr )
           : ReadOnlyHandle(  prefix_name,   interface_name,   value_ptr )
           {
           }
          
           explicit ReadWriteHandle(  const std::string & interface_name ) : ReadOnlyHandle(  interface_name ) {}
          
           explicit ReadWriteHandle(  const char * interface_name ) : ReadOnlyHandle(  interface_name ) {}
          
           ReadWriteHandle(  const ReadWriteHandle & other ) = default;
          
           ReadWriteHandle(  ReadWriteHandle && other ) = default;
          
           ReadWriteHandle & operator=(  const ReadWriteHandle & other ) = default;
          
           ReadWriteHandle & operator=(  ReadWriteHandle && other ) = default;
          
           virtual ~ReadWriteHandle(   ) = default;
          
           void set_value(  double value )
           {
           THROW_ON_NULLPTR(  this->value_ptr_ );
           *this->value_ptr_ = value;
           }
          };
          
          class StateInterface : public ReadOnlyHandle
          {
          public:
           StateInterface(  const StateInterface & other ) = default;
          
           StateInterface(  StateInterface && other ) = default;
          
           using ReadOnlyHandle::ReadOnlyHandle;
          };
          
          class CommandInterface : public ReadWriteHandle
          {
          public:
           /// CommandInterface copy constructor is actively deleted.
           /**
           * Command interfaces are having a unique ownership and thus
           * can't be copied in order to avoid simultaneous writes to
           * the same resource.
           */
           CommandInterface(  const CommandInterface & other ) = delete;
          
           CommandInterface(  CommandInterface && other ) = default;
          
           using ReadWriteHandle::ReadWriteHandle;
          };
          
          } // namespace hardware_interface
          
          #endif // HARDWARE_INTERFACE__HANDLE_HPP_

ros2_control/hardware_interface/include/hardware_interface/hardware_component_info.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          //
          // Author: Denis Stogl
          //
          
          #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_
          #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          /// Hardware Component Information
          /**
           * This struct contains information about a given hardware component.
           */
          struct HardwareComponentInfo
          {
           /// Component name.
           std::string name;
          
           /// Component "classification": "actuator",   "sensor" or "system"
           std::string type;
          
           /// Component class type.
           std::string class_type;
          
           /// Component current state.
           rclcpp_lifecycle::State state;
          
           /// List of provided state interfaces by the component.
           std::vector<std::string> state_interfaces;
          
           /// List of provided command interfaces by the component.
           std::vector<std::string> command_interfaces;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_

ros2_control/hardware_interface/include/hardware_interface/hardware_info.hpp

       1  // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
          #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
          
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          namespace hardware_interface
          {
          /**
           * This structure stores information about components defined for a specific hardware
           * in robot's URDF.
           */
          struct InterfaceInfo
          {
           /**
           * Name of the command interfaces that can be set,   e.g. "position",   "velocity",   etc.
           * Used by joints and GPIOs.
           */
           std::string name;
           /// (  Optional ) Minimal allowed values of the interface.
           std::string min;
           /// (  Optional ) Maximal allowed values of the interface.
           std::string max;
           /// (  Optional ) Initial value of the interface.
           std::string initial_value;
           /// (  Optional ) The datatype of the interface,   e.g. "bool",   "int". Used by GPIOs.
           std::string data_type;
           /// (  Optional ) If the handle is an array,   the size of the array. Used by GPIOs.
           int size;
          };
          
          /**
           * This structure stores information about components defined for a specific hardware
           * in robot's URDF.
           */
          struct ComponentInfo
          {
           /// Name of the component.
           std::string name;
           /// Type of the component: sensor,   joint,   or GPIO.
           std::string type;
           /**
           * Name of the command interfaces that can be set,   e.g. "position",   "velocity",   etc.
           * Used by joints and GPIOs.
           */
           std::vector<InterfaceInfo> command_interfaces;
           /**
           * Name of the state interfaces that can be read,   e.g. "position",   "velocity",   etc.
           * Used by joints,   sensors and GPIOs.
           */
           std::vector<InterfaceInfo> state_interfaces;
           /// (  Optional ) Key-value pairs of component parameters,   e.g. min/max values or serial number.
           std::unordered_map<std::string,   std::string> parameters;
          };
          
          /// Contains semantic info about a given joint loaded from URDF for a transmission
          struct JointInfo
          {
           std::string name;
           std::vector<std::string> interfaces;
           std::string role;
           double mechanical_reduction = 1.0;
           double offset = 0.0;
          };
          
          /// Contains semantic info about a given actuator loaded from URDF for a transmission
          struct ActuatorInfo
          {
           std::string name;
           std::vector<std::string> interfaces;
           std::string role;
           double mechanical_reduction = 1.0;
           double offset = 0.0;
          };
          
          /// Contains semantic info about a given transmission loaded from URDF
          struct TransmissionInfo
          {
           std::string name;
           std::string type;
           std::vector<JointInfo> joints;
           std::vector<ActuatorInfo> actuators;
           /// (  Optional ) Key-value pairs of custom parameters
           std::unordered_map<std::string,   std::string> parameters;
          };
          
          /// This structure stores information about hardware defined in a robot's URDF.
          struct HardwareInfo
          {
           /// Name of the hardware.
           std::string name;
           /// Type of the hardware: actuator,   sensor or system.
           std::string type;
           /// Class of the hardware that will be dynamically loaded.
           std::string hardware_class_type;
           /// (  Optional ) Key-value pairs for hardware parameters.
           std::unordered_map<std::string,   std::string> hardware_parameters;
           /**
           * Map of joints provided by the hardware where the key is the joint name.
           * Required for Actuator and System Hardware.
           */
           std::vector<ComponentInfo> joints;
           /**
           * Map of sensors provided by the hardware where the key is the joint or link name.
           * Required for Sensor and optional for System Hardware.
           */
           std::vector<ComponentInfo> sensors;
           /**
           * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO.
           * Optional for any hardware components.
           */
           std::vector<ComponentInfo> gpios;
           /**
           * Map of transmissions to calculate ration between joints and physical actuators.
           * Optional for Actuator and System Hardware.
           */
           std::vector<TransmissionInfo> transmissions;
           /**
           * The XML contents prior to parsing
           */
           std::string original_xml;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_

ros2_control/hardware_interface/include/hardware_interface/loaned_command_interface.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
          #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
          
          #include <functional>
          #include <string>
          #include <utility>
          
          #include "hardware_interface/handle.hpp"
          
          namespace hardware_interface
          {
      26  class LoanedCommandInterface
          {
          public:
           using Deleter = std::function<void(  void )>;
          
           explicit LoanedCommandInterface(  CommandInterface & command_interface )
           : LoanedCommandInterface(  command_interface,   nullptr )
           {
           }
          
           LoanedCommandInterface(  CommandInterface & command_interface,   Deleter && deleter )
           : command_interface_(  command_interface ),   deleter_(  std::forward<Deleter>(  deleter ) )
           {
           }
          
      41   LoanedCommandInterface(  const LoanedCommandInterface & other ) = delete;
          
      43   LoanedCommandInterface(  LoanedCommandInterface && other ) = default;
          
           virtual ~LoanedCommandInterface(   )
           {
           if (  deleter_ )
           {
           deleter_(   );
           }
           }
          
           const std::string get_name(   ) const { return command_interface_.get_name(   ); }
          
           const std::string & get_interface_name(   ) const { return command_interface_.get_interface_name(   ); }
          
           [[deprecated(  
           "Replaced by get_name method,   which is semantically more correct" )]] const std::string
           get_full_name(   ) const
           {
           return command_interface_.get_name(   );
           }
          
           const std::string & get_prefix_name(   ) const { return command_interface_.get_prefix_name(   ); }
          
           void set_value(  double val ) { command_interface_.set_value(  val ); }
          
           double get_value(   ) const { return command_interface_.get_value(   ); }
          
          protected:
           CommandInterface & command_interface_;
           Deleter deleter_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_

ros2_control/hardware_interface/include/hardware_interface/loaned_state_interface.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
          #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
          
          #include <functional>
          #include <string>
          #include <utility>
          
          #include "hardware_interface/handle.hpp"
          
          namespace hardware_interface
          {
      26  class LoanedStateInterface
          {
          public:
           using Deleter = std::function<void(  void )>;
          
           explicit LoanedStateInterface(  StateInterface & state_interface )
           : LoanedStateInterface(  state_interface,   nullptr )
           {
           }
          
           LoanedStateInterface(  StateInterface & state_interface,   Deleter && deleter )
           : state_interface_(  state_interface ),   deleter_(  std::forward<Deleter>(  deleter ) )
           {
           }
          
      41   LoanedStateInterface(  const LoanedStateInterface & other ) = delete;
          
      43   LoanedStateInterface(  LoanedStateInterface && other ) = default;
          
           virtual ~LoanedStateInterface(   )
           {
           if (  deleter_ )
           {
           deleter_(   );
           }
           }
          
           const std::string get_name(   ) const { return state_interface_.get_name(   ); }
          
           const std::string & get_interface_name(   ) const { return state_interface_.get_interface_name(   ); }
          
           [[deprecated(  
           "Replaced by get_name method,   which is semantically more correct" )]] const std::string
           get_full_name(   ) const
           {
           return state_interface_.get_name(   );
           }
          
           const std::string & get_prefix_name(   ) const { return state_interface_.get_prefix_name(   ); }
          
           double get_value(   ) const { return state_interface_.get_value(   ); }
          
          protected:
           StateInterface & state_interface_;
           Deleter deleter_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_

ros2_control/hardware_interface/include/hardware_interface/macros.hpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__MACROS_HPP_
          #define HARDWARE_INTERFACE__MACROS_HPP_
          
          #include <stdexcept>
          #include <string>
          
          #include "rcpputils/pointer_traits.hpp"
          
          #ifdef _WIN32
          #define __PRETTY_FUNCTION__ __FUNCTION__
          #endif
          
          #define THROW_ON_NULLPTR(  pointer ) \
           static_assert(   \
           rcpputils::is_pointer<typename std::remove_reference<decltype(  pointer )>::type>::value,   \
           #pointer " has to be a pointer" ); \
           if (  !pointer ) \
           { \
           throw std::runtime_error(  std::string(  __PRETTY_FUNCTION__ ) + " failed. " #pointer " is null." ); \
           }
          
          #define THROW_ON_NOT_NULLPTR(  pointer ) \
           static_assert(   \
           rcpputils::is_pointer<typename std::remove_reference<decltype(  pointer )>::type>::value,   \
           #pointer " has to be a pointer" ); \
           if (  pointer ) \
           { \
           throw std::runtime_error(   \
           std::string(  __PRETTY_FUNCTION__ ) + " failed. " #pointer " would leak memory" ); \
           }
          
          #endif // HARDWARE_INTERFACE__MACROS_HPP_

ros2_control/hardware_interface/include/hardware_interface/resource_manager.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
          #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
          
          #include <memory>
          #include <mutex>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/hardware_component_info.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          
          namespace hardware_interface
          {
      34  class ActuatorInterface;
      35  class SensorInterface;
      36  class SystemInterface;
      37  class ResourceStorage;
          
          class HARDWARE_INTERFACE_PUBLIC ResourceManager
          {
          public:
           /// Default constructor for the Resource Manager.
      43   ResourceManager(   );
          
           /// Constructor for the Resource Manager.
           /**
           * The implementation loads the specified urdf and initializes the
           * hardware components listed within as well as populate their respective
           * state and command interfaces.
           *
           * If the interfaces ought to be validated,   the constructor throws an exception
           * in case the URDF lists interfaces which are not available.
           *
           * \param[in] urdf string containing the URDF.
           * \param[in] validate_interfaces boolean argument indicating whether the exported
           * interfaces ought to be validated. Defaults to true.
           * \param[in] activate_all boolean argument indicating if all resources should be immediately
           * activated. Currently used only in tests. In typical applications use parameters
           * "autostart_components" and "autoconfigure_components" instead.
           */
      61   explicit ResourceManager(  
           const std::string & urdf,   bool validate_interfaces = true,   bool activate_all = false );
          
      64   ResourceManager(  const ResourceManager & ) = delete;
          
      66   ~ResourceManager(   );
          
           /// Load resources from on a given URDF.
           /**
           * The resource manager can be post initialized with a given URDF.
           * This is mainly used in conjunction with the default constructor
           * in which the URDF might not be present at first initialization.
           *
           * \param[in] urdf string containing the URDF.
           * \param[in] validate_interfaces boolean argument indicating whether the exported
           * interfaces ought to be validated. Defaults to true.
           */
      78   void load_urdf(  const std::string & urdf,   bool validate_interfaces = true );
          
           /// Claim a state interface given its key.
           /**
           * The resource is claimed as long as being in scope.
           * Once the resource is going out of scope,   the destructor
           * returns.
           *
           * \param[in] key String identifier which state interface to claim
           * \return state interface
           */
      89   LoanedStateInterface claim_state_interface(  const std::string & key );
          
           /// Returns all registered state interfaces keys.
           /**
           * The keys are collected from each loaded hardware component.
           * \return Vector of strings,   containing all registered keys.
           */
      96   std::vector<std::string> state_interface_keys(   ) const;
          
           /// Returns all available state interfaces keys.
           /**
           * The keys are collected from the available list.
           * \return Vector of strings,   containing all available state interface names.
           */
     103   std::vector<std::string> available_state_interfaces(   ) const;
          
           /// Checks whether a state interface is registered under the given key.
           /**
           * \return true if interface exist,   false otherwise.
           */
     109   bool state_interface_exists(  const std::string & key ) const;
          
           /// Checks whether a state interface is available under the given key.
           /**
           * \return true if interface is available,   false otherwise.
           */
     115   bool state_interface_is_available(  const std::string & name ) const;
          
           /// Add controllers' reference interfaces to resource manager.
           /**
           * Interface for transferring management of reference interfaces to resource manager.
           * When chaining controllers,   reference interfaces are used as command interface of preceding
           * controllers.
           * Therefore,   they should be managed in the same way as command interface of hardware.
           *
           * \param[in] controller_name name of the controller which reference interfaces are imported.
           * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces.
           */
     127   void import_controller_reference_interfaces(  
           const std::string & controller_name,   std::vector<CommandInterface> & interfaces );
          
           /// Get list of reference interface of a controller.
           /**
           * Returns lists of stored reference interfaces names for a controller.
           *
           * \param[in] controller_name for which list of reference interface names is returned.
           * \returns list of reference interface names.
           */
     137   std::vector<std::string> get_controller_reference_interface_names(  
           const std::string & controller_name );
          
           /// Add controller's reference interface to available list.
           /**
           * Adds interfaces of a controller with given name to the available list. This method should be
           * called when a controller gets activated with chained mode turned on. That means,   the
           * controller's reference interfaces can be used by another controller in chained architectures.
           *
           * \param[in] controller_name name of the controller which interfaces should become available.
           */
     148   void make_controller_reference_interfaces_available(  const std::string & controller_name );
          
           /// Remove controller's reference interface to available list.
           /**
           * Removes interfaces of a controller with given name from the available list. This method should
           * be called when a controller gets deactivated and its reference interfaces cannot be used by
           * another controller anymore.
           *
           * \param[in] controller_name name of the controller which interfaces should become unavailable.
           */
     158   void make_controller_reference_interfaces_unavailable(  const std::string & controller_name );
          
           /// Remove controllers reference interfaces from resource manager.
           /**
           * Remove reference interfaces from resource manager,   i.e.,   resource storage.
           * The interfaces will be deleted from all internal maps and lists.
           *
           * \param[in] controller_name list of interface names that will be deleted from resource manager.
           */
     167   void remove_controller_reference_interfaces(  const std::string & controller_name );
          
           /// Checks whether a command interface is already claimed.
           /**
           * Any command interface can only be claimed by a single instance.
           * \note the equivalent function does not exist for state interfaces.
           * These are solely read-only and can thus be used by multiple instances.
           *
           * \param[in] key string identifying the interface to check.
           * \return true if interface is already claimed,   false if available.
           */
     178   bool command_interface_is_claimed(  const std::string & key ) const;
          
           /// Claim a command interface given its key.
           /**
           * The resource is claimed as long as being in scope.
           * Once the resource is going out of scope,   the destructor
           * returns and thus frees the resource to claimed by others.
           *
           * \param[in] key String identifier which command interface to claim
           * \return command interface
           */
     189   LoanedCommandInterface claim_command_interface(  const std::string & key );
          
           /// Returns all registered command interfaces keys.
           /**
           * The keys are collected from each loaded hardware component.
           * \return vector of strings,   containing all registered keys.
           */
     196   std::vector<std::string> command_interface_keys(   ) const;
          
           /// Returns all available command interfaces keys.
           /**
           * The keys are collected from the available list.
           * \return vector of strings,   containing all available command interface names.
           */
     203   std::vector<std::string> available_command_interfaces(   ) const;
          
           /// Checks whether a command interface is registered under the given key.
           /**
           * \param[in] key string identifying the interface to check.
           * \return true if interface exist,   false otherwise.
           */
     210   bool command_interface_exists(  const std::string & key ) const;
          
           /// Checks whether a command interface is available under the given name.
           /**
           * \param[in] name string identifying the interface to check.
           * \return true if interface is available,   false otherwise.
           */
     217   bool command_interface_is_available(  const std::string & interface ) const;
          
           /// Return the number size_t of loaded actuator components.
           /**
           * \return number of actuator components.
           */
     223   size_t actuator_components_size(   ) const;
          
           /// Return the number of loaded sensor components.
           /**
           * \return number of sensor components.
           */
     229   size_t sensor_components_size(   ) const;
          
           /// Return the number of loaded system components.
           /**
           * \return size_t number of system components.
           */
     235   size_t system_components_size(   ) const;
          
           /// Import a hardware component which is not listed in the URDF
           /**
           * Components which are initialized outside a URDF can be added post initialization.
           * Nevertheless,   there should still be `HardwareInfo` available for this component,  
           * either parsed from a URDF string (  easiest ) or filled manually.
           *
           * \note this might invalidate existing state and command interfaces and should thus
           * not be called when a controller is running.
           * \note given that no hardware_info is available,   the component has to be configured
           * externally and prior to the call to import.
           * \param[in] actuator pointer to the actuator interface.
           * \param[in] hardware_info hardware info
           */
     250   void import_component(  
           std::unique_ptr<ActuatorInterface> actuator,   const HardwareInfo & hardware_info );
          
           /// Import a hardware component which is not listed in the URDF
           /**
           * Components which are initialized outside a URDF can be added post initialization.
           * Nevertheless,   there should still be `HardwareInfo` available for this component,  
           * either parsed from a URDF string (  easiest ) or filled manually.
           *
           * \note this might invalidate existing state and command interfaces and should thus
           * not be called when a controller is running.
           * \note given that no hardware_info is available,   the component has to be configured
           * externally and prior to the call to import.
           * \param[in] sensor pointer to the sensor interface.
           * \param[in] hardware_info hardware info
           */
     266   void import_component(  
           std::unique_ptr<SensorInterface> sensor,   const HardwareInfo & hardware_info );
          
           /// Import a hardware component which is not listed in the URDF
           /**
           * Components which are initialized outside a URDF can be added post initialization.
           * Nevertheless,   there should still be `HardwareInfo` available for this component,  
           * either parsed from a URDF string (  easiest ) or filled manually.
           *
           * \note this might invalidate existing state and command interfaces and should thus
           * not be called when a controller is running.
           * \note given that no hardware_info is available,   the component has to be configured
           * externally and prior to the call to import.
           * \param[in] system pointer to the system interface.
           * \param[in] hardware_info hardware info
           */
     282   void import_component(  
           std::unique_ptr<SystemInterface> system,   const HardwareInfo & hardware_info );
          
           /// Return status for all components.
           /**
           * \return map of hardware names and their status.
           */
     289   std::unordered_map<std::string,   HardwareComponentInfo> get_components_status(   );
          
           /// Prepare the hardware components for a new command interface mode
           /**
           * Hardware components are asked to prepare a new command interface claim.
           *
           * \note this is intended for mode-switching when a hardware interface needs to change
           * control mode depending on which command interface is claimed.
           * \note this is for non-realtime preparing for and accepting new command resource
           * combinations.
           * \note accept_command_resource_claim is called on all actuators and system components
           * and hardware interfaces should return hardware_interface::return_type::OK
           * by default
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
           * \return true if switch can be prepared,   false if a component rejects switch request.
           */
     306   bool prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           /// Notify the hardware components that realtime hardware mode switching should occur.
           /**
           * Hardware components are asked to perform the command interface mode switching.
           *
           * \note this is intended for mode-switching when a hardware interface needs to change
           * control mode depending on which command interface is claimed.
           * \note this is for realtime switching of the command interface.
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
           * \return true if switch is performed,   false if a component rejects switching.
           */
     321   bool perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           /// Sets state of hardware component.
           /**
           * Set set of hardware component if possible.
           * Takes care of all transitions needed to reach the target state.
           * It implements the state machine from: https://design.ros2.org/articles/node_lifecycle.html
           *
           * The method is not part of the real-time critical update loop.
           *
           * \param[in] component_name component name to change state.
           * \param[in] target_state target state to set for a hardware component.
           * \return hardware_interface::retun_type::OK if component successfully switched its state and
           * hardware_interface::return_type::ERROR any of state transitions has failed.
           */
     338   return_type set_component_state(  
           const std::string & component_name,   rclcpp_lifecycle::State & target_state );
          
           /// Reads all loaded hardware components.
           /**
           * Reads from all active hardware components.
           *
           * Part of the real-time critical update loop.
           * It is realtime-safe if used hadware interfaces are implemented adequately.
           */
     348   void read(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           /// Write all loaded hardware components.
           /**
           * Writes to all active hardware components.
           *
           * Part of the real-time critical update loop.
           * It is realtime-safe if used hadware interfaces are implemented adequately.
           */
     357   void write(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           /// Activates all available hardware components in the system.
           /**
           * All available hardware components int the ros2_control framework are activated.
           * This is used to preserve default behavior from previous versions where all hardware components
           * are activated per default.
           */
     365   void activate_all_components(   );
          
          private:
           void validate_storage(  const std::vector<hardware_interface::HardwareInfo> & hardware_info ) const;
          
           void release_command_interface(  const std::string & key );
          
           std::unordered_map<std::string,   bool> claimed_command_interface_map_;
          
           mutable std::recursive_mutex resource_interfaces_lock_;
           mutable std::recursive_mutex claimed_command_interfaces_lock_;
           std::unique_ptr<ResourceStorage> resource_storage_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_

ros2_control/hardware_interface/include/hardware_interface/sensor.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__SENSOR_HPP_
          #define HARDWARE_INTERFACE__SENSOR_HPP_
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/visibility_control.h"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
      33  class SensorInterface;
          
          class Sensor final
          {
          public:
      38   Sensor(   ) = default;
          
           HARDWARE_INTERFACE_PUBLIC
           explicit Sensor(  std::unique_ptr<SensorInterface> impl );
          
      43   Sensor(  Sensor && other ) = default;
          
      45   ~Sensor(   ) = default;
          
      47   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & initialize(  const HardwareInfo & sensor_info );
          
      50   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & configure(   );
          
           HARDWARE_INTERFACE_PUBLIC
      54   const rclcpp_lifecycle::State & cleanup(   );
          
      56   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & shutdown(   );
          
      59   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & activate(   );
          
           HARDWARE_INTERFACE_PUBLIC
      63   const rclcpp_lifecycle::State & deactivate(   );
          
      65   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & error(   );
          
           HARDWARE_INTERFACE_PUBLIC
           std::vector<StateInterface> export_state_interfaces(   );
          
           HARDWARE_INTERFACE_PUBLIC
      72   std::string get_name(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
      75   const rclcpp_lifecycle::State & get_state(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
           return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
          private:
           std::unique_ptr<SensorInterface> impl_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__SENSOR_HPP_

ros2_control/hardware_interface/include/hardware_interface/sensor_interface.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
          #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          /// Virtual Class to implement when integrating a stand-alone sensor into ros2_control.
          /**
           * The typical examples are Force-Torque Sensor (  FTS ),   Interial Measurement Unit (  IMU ).
           *
           * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
           * with the following meaning:
           *
           * \returns CallbackReturn::SUCCESS method execution was successful.
           * \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
           * \returns CallbackReturn::ERROR critical error has happened that should be managed in
           * "on_error" method.
           *
           * The hardware ends after each method in a state with the following meaning:
           *
           * UNCONFIGURED (  on_init,   on_cleanup ):
           * Hardware is initialized but communication is not started and therefore no interface is available.
           *
           * INACTIVE (  on_configure,   on_deactivate ):
           * Communication with the hardware is started and it is configured.
           * States can be read and non-movement hardware interfaces commanded.
           * Hardware interfaces for movement will NOT be available.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           *
           * FINALIZED (  on_shutdown ):
           * Hardware interface is ready for unloading/destruction.
           * Allocated memory is cleaned up.
           *
           * ACTIVE (  on_activate ):
           * Power circuits of hardware are active and hardware can be moved,   e.g.,   brakes are disabled.
           * Command interfaces for movement are available and have to be accepted.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           */
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      69  class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
          {
          public:
      72   SensorInterface(   )
           : lifecycle_state_(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN,   lifecycle_state_names::UNKNOWN ) )
           {
           }
          
           /// SensorInterface copy constructor is actively deleted.
           /**
           * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
           * failed or simultaneous access to hardware.
           */
           SensorInterface(  const SensorInterface & other ) = delete;
          
           SensorInterface(  SensorInterface && other ) = default;
          
           virtual ~SensorInterface(   ) = default;
          
           /// Initialization of the hardware interface from data parsed from the robot's URDF.
           /**
           * \param[in] hardware_info structure with data from URDF.
           * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
           * \returns CallbackReturn::ERROR if any error happens or data are missing.
           */
           virtual CallbackReturn on_init(  const HardwareInfo & hardware_info )
           {
           info_ = hardware_info;
           return CallbackReturn::SUCCESS;
           };
          
           /// Exports all state interfaces for this hardware interface.
           /**
           * The state interfaces have to be created and transferred according
           * to the hardware info passed in for the configuration.
           *
           * Note the ownership over the state interfaces is transferred to the caller.
           *
           * \return vector of state interfaces
           */
     110   virtual std::vector<StateInterface> export_state_interfaces(   ) = 0;
          
           /// Read the current state values from the actuator.
           /**
           * The data readings from the physical hardware has to be updated
           * and reflected accordingly in the exported state interfaces.
           * That is,   the data pointed by the interfaces shall be updated.
           *
           * \return return_type::OK if the read was successful,   return_type::ERROR otherwise.
           */
           virtual return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Get name of the actuator hardware.
           /**
           * \return name.
           */
           virtual std::string get_name(   ) const { return info_.name; }
          
           /// Get life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           const rclcpp_lifecycle::State & get_state(   ) const { return lifecycle_state_; }
          
           /// Set life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           void set_state(  const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
          
          protected:
           HardwareInfo info_;
           rclcpp_lifecycle::State lifecycle_state_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_

ros2_control/hardware_interface/include/hardware_interface/system.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__SYSTEM_HPP_
          #define HARDWARE_INTERFACE__SYSTEM_HPP_
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/visibility_control.h"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
      33  class SystemInterface;
          
          class System final
          {
          public:
           HARDWARE_INTERFACE_PUBLIC
           explicit System(  std::unique_ptr<SystemInterface> impl );
          
      41   System(  System && other ) = default;
          
      43   ~System(   ) = default;
          
      45   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & initialize(  const HardwareInfo & system_info );
          
      48   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & configure(   );
          
           HARDWARE_INTERFACE_PUBLIC
      52   const rclcpp_lifecycle::State & cleanup(   );
          
      54   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & shutdown(   );
          
      57   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & activate(   );
          
           HARDWARE_INTERFACE_PUBLIC
      61   const rclcpp_lifecycle::State & deactivate(   );
          
      63   HARDWARE_INTERFACE_PUBLIC
           const rclcpp_lifecycle::State & error(   );
          
           HARDWARE_INTERFACE_PUBLIC
           std::vector<StateInterface> export_state_interfaces(   );
          
           HARDWARE_INTERFACE_PUBLIC
           std::vector<CommandInterface> export_command_interfaces(   );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces );
          
           HARDWARE_INTERFACE_PUBLIC
      83   std::string get_name(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
      86   const rclcpp_lifecycle::State & get_state(   ) const;
          
           HARDWARE_INTERFACE_PUBLIC
           return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
           HARDWARE_INTERFACE_PUBLIC
           return_type write(  const rclcpp::Time & time,   const rclcpp::Duration & period );
          
          private:
           std::unique_ptr<SystemInterface> impl_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__SYSTEM_HPP_

ros2_control/hardware_interface/include/hardware_interface/system_interface.hpp

          // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
          #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          /// Virtual Class to implement when integrating a complex system into ros2_control.
          /**
           * The common examples for these types of hardware are multi-joint systems with or without sensors
           * such as industrial or humanoid robots.
           *
           * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
           * with the following meaning:
           *
           * \returns CallbackReturn::SUCCESS method execution was successful.
           * \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
           * \returns CallbackReturn::ERROR critical error has happened that should be managed in
           * "on_error" method.
           *
           * The hardware ends after each method in a state with the following meaning:
           *
           * UNCONFIGURED (  on_init,   on_cleanup ):
           * Hardware is initialized but communication is not started and therefore no interface is available.
           *
           * INACTIVE (  on_configure,   on_deactivate ):
           * Communication with the hardware is started and it is configured.
           * States can be read and non-movement hardware interfaces commanded.
           * Hardware interfaces for movement will NOT be available.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           *
           * FINALIZED (  on_shutdown ):
           * Hardware interface is ready for unloading/destruction.
           * Allocated memory is cleaned up.
           *
           * ACTIVE (  on_activate ):
           * Power circuits of hardware are active and hardware can be moved,   e.g.,   brakes are disabled.
           * Command interfaces for movement are available and have to be accepted.
           * Those interfaces are: HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_ACCELERATION,   and HW_IF_EFFORT.
           */
          
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      70  class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
          {
          public:
      73   SystemInterface(   )
           : lifecycle_state_(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN,   lifecycle_state_names::UNKNOWN ) )
           {
           }
          
           /// SystemInterface copy constructor is actively deleted.
           /**
           * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
           * failed or simultaneous access to hardware.
           */
           SystemInterface(  const SystemInterface & other ) = delete;
          
           SystemInterface(  SystemInterface && other ) = default;
          
           virtual ~SystemInterface(   ) = default;
          
           /// Initialization of the hardware interface from data parsed from the robot's URDF.
           /**
           * \param[in] hardware_info structure with data from URDF.
           * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
           * \returns CallbackReturn::ERROR if any error happens or data are missing.
           */
           virtual CallbackReturn on_init(  const HardwareInfo & hardware_info )
           {
           info_ = hardware_info;
           return CallbackReturn::SUCCESS;
           };
          
           /// Exports all state interfaces for this hardware interface.
           /**
           * The state interfaces have to be created and transferred according
           * to the hardware info passed in for the configuration.
           *
           * Note the ownership over the state interfaces is transferred to the caller.
           *
           * \return vector of state interfaces
           */
     111   virtual std::vector<StateInterface> export_state_interfaces(   ) = 0;
          
           /// Exports all command interfaces for this hardware interface.
           /**
           * The command interfaces have to be created and transferred according
           * to the hardware info passed in for the configuration.
           *
           * Note the ownership over the state interfaces is transferred to the caller.
           *
           * \return vector of command interfaces
           */
           virtual std::vector<CommandInterface> export_command_interfaces(   ) = 0;
          
           /// Prepare for a new command interface switch.
           /**
           * Prepare for any mode-switching required by the new command interface combination.
           *
           * \note This is a non-realtime evaluation of whether a set of command interface claims are
           * possible,   and call to start preparing data structures for the upcoming switch that will occur.
           * \note All starting and stopping interface keys are passed to all components,   so the function should
           * return return_type::OK by default when given interface keys not relevant for this component.
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
           * \return return_type::OK if the new command interface combination can be prepared,  
           * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
           */
           virtual return_type prepare_command_mode_switch(  
           const std::vector<std::string> & /*start_interfaces*/,  
           const std::vector<std::string> & /*stop_interfaces*/ )
           {
           return return_type::OK;
           }
          
           // Perform switching to the new command interface.
           /**
           * Perform the mode-switching for the new command interface combination.
           *
           * \note This is part of the realtime update loop,   and should be fast.
           * \note All starting and stopping interface keys are passed to all components,   so the function should
           * return return_type::OK by default when given interface keys not relevant for this component.
           * \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
           * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
           * \return return_type::OK if the new command interface combination can be switched to,  
           * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
           */
           virtual return_type perform_command_mode_switch(  
           const std::vector<std::string> & /*start_interfaces*/,  
           const std::vector<std::string> & /*stop_interfaces*/ )
           {
           return return_type::OK;
           }
          
           /// Read the current state values from the actuator.
           /**
           * The data readings from the physical hardware has to be updated
           * and reflected accordingly in the exported state interfaces.
           * That is,   the data pointed by the interfaces shall be updated.
           *
           * \return return_type::OK if the read was successful,   return_type::ERROR otherwise.
           */
           virtual return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Write the current command values to the actuator.
           /**
           * The physical hardware shall be updated with the latest value from
           * the exported command interfaces.
           *
           * \return return_type::OK if the read was successful,   return_type::ERROR otherwise.
           */
           virtual return_type write(  const rclcpp::Time & time,   const rclcpp::Duration & period ) = 0;
          
           /// Get name of the actuator hardware.
           /**
           * \return name.
           */
           virtual std::string get_name(   ) const { return info_.name; }
          
           /// Get life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           const rclcpp_lifecycle::State & get_state(   ) const { return lifecycle_state_; }
          
           /// Set life-cycle state of the actuator hardware.
           /**
           * \return state.
           */
           void set_state(  const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
          
          protected:
           HardwareInfo info_;
           rclcpp_lifecycle::State lifecycle_state_;
          };
          
          } // namespace hardware_interface
          #endif // HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_

ros2_control/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_
          #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_
          
          #include <cstdint>
          
          namespace hardware_interface
          {
      22  enum class return_type : std::uint8_t
          {
           OK = 0,  
           ERROR = 1,  
          };
          
          } // namespace hardware_interface
          
          #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_

ros2_control/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

       1  // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
          #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
          
          namespace hardware_interface
          {
          /// Constant defining position interface
      21  constexpr char HW_IF_POSITION[] = "position";
          /// Constant defining velocity interface
      23  constexpr char HW_IF_VELOCITY[] = "velocity";
          /// Constant defining acceleration interface
          constexpr char HW_IF_ACCELERATION[] = "acceleration";
          /// Constant defining effort interface
      27  constexpr char HW_IF_EFFORT[] = "effort";
          } // namespace hardware_interface
          
          #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_

ros2_control/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          /// \author: Denis Stogl
          
          #ifndef HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_
          #define HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_
          
          #include <string>
          
          namespace hardware_interface
          {
          namespace lifecycle_state_names
          {
          /// Constants defining string labels corresponding to lifecycle states
      27  constexpr char UNKNOWN[] = "unknown";
      28  constexpr char UNCONFIGURED[] = "unconfigured";
          constexpr char INACTIVE[] = "inactive";
      30  constexpr char ACTIVE[] = "active";
      31  constexpr char FINALIZED[] = "finalized";
          } // namespace lifecycle_state_names
          
          } // namespace hardware_interface
          
          #endif // HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_

ros2_control/hardware_interface/include/hardware_interface/visibility_control.h

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
          #define HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define HARDWARE_INTERFACE_EXPORT __attribute__(  (  dllexport ) )
          #define HARDWARE_INTERFACE_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define HARDWARE_INTERFACE_EXPORT __declspec(  dllexport )
          #define HARDWARE_INTERFACE_IMPORT __declspec(  dllimport )
          #endif
          #ifdef HARDWARE_INTERFACE_BUILDING_DLL
          #define HARDWARE_INTERFACE_PUBLIC HARDWARE_INTERFACE_EXPORT
          #else
          #define HARDWARE_INTERFACE_PUBLIC HARDWARE_INTERFACE_IMPORT
          #endif
          #define HARDWARE_INTERFACE_PUBLIC_TYPE HARDWARE_INTERFACE_PUBLIC
          #define HARDWARE_INTERFACE_LOCAL
          #else
          #define HARDWARE_INTERFACE_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define HARDWARE_INTERFACE_IMPORT
          #if __GNUC__ >= 4
          #define HARDWARE_INTERFACE_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define HARDWARE_INTERFACE_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define HARDWARE_INTERFACE_PUBLIC
          #define HARDWARE_INTERFACE_LOCAL
          #endif
          #define HARDWARE_INTERFACE_PUBLIC_TYPE
          #endif
          
          #endif // HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_

ros2_control/hardware_interface/include/mock_components/generic_system.hpp

          // Copyright (  c ) 2021 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Author: Jafar Abdi,   Denis Stogl
          
          #ifndef MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_
          #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_
          
          #include <string>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::return_type;
          
          namespace mock_components
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface
          {
          public:
      38   CallbackReturn on_init(  const hardware_interface::HardwareInfo & info ) override;
          
      40   std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override;
          
      42   std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override;
          
      44   return_type read(  const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
      46   return_type write(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
          protected:
           /// Use standard interfaces for joints because they are relevant for dynamic behavior
           /**
           * By splitting the standard interfaces from other type,   the users are able to inherit this
           * class and simply create small "simulation" with desired dynamic behavior.
           * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and
           * controllers.
           */
           const std::vector<std::string> standard_interfaces_ = {
           hardware_interface::HW_IF_POSITION,   hardware_interface::HW_IF_VELOCITY,  
           hardware_interface::HW_IF_ACCELERATION,   hardware_interface::HW_IF_EFFORT};
          
           const size_t POSITION_INTERFACE_INDEX = 0;
          
           struct MimicJoint
           {
           std::size_t joint_index;
           std::size_t mimicked_joint_index;
           double multiplier = 1.0;
           };
           std::vector<MimicJoint> mimic_joints_;
          
           /// The size of this vector is (  standard_interfaces_.size(   ) x nr_joints )
           std::vector<std::vector<double>> joint_commands_;
           std::vector<std::vector<double>> joint_states_;
          
           std::vector<std::string> other_interfaces_;
           /// The size of this vector is (  other_interfaces_.size(   ) x nr_joints )
           std::vector<std::vector<double>> other_commands_;
           std::vector<std::vector<double>> other_states_;
          
           std::vector<std::string> sensor_interfaces_;
           /// The size of this vector is (  sensor_interfaces_.size(   ) x nr_joints )
           std::vector<std::vector<double>> sensor_fake_commands_;
           std::vector<std::vector<double>> sensor_states_;
          
           std::vector<std::string> gpio_interfaces_;
           /// The size of this vector is (  gpio_interfaces_.size(   ) x nr_joints )
           std::vector<std::vector<double>> gpio_fake_commands_;
           std::vector<std::vector<double>> gpio_commands_;
           std::vector<std::vector<double>> gpio_states_;
          
          private:
           template <typename HandleType>
           bool get_interface(  
           const std::string & name,   const std::vector<std::string> & interface_list,  
           const std::string & interface_name,   const size_t vector_index,  
           std::vector<std::vector<double>> & values,   std::vector<HandleType> & interfaces );
          
           void initialize_storage_vectors(  
           std::vector<std::vector<double>> & commands,   std::vector<std::vector<double>> & states,  
           const std::vector<std::string> & interfaces );
          
           template <typename InterfaceType>
           bool populate_interfaces(  
           const std::vector<hardware_interface::ComponentInfo> & components,  
           std::vector<std::string> & interfaces,   std::vector<std::vector<double>> & storage,  
           std::vector<InterfaceType> & target_interfaces,   bool using_state_interfaces );
          
           bool use_fake_gpio_command_interfaces_;
           bool use_fake_sensor_command_interfaces_;
          
           double position_state_following_offset_;
           std::string custom_interface_with_following_offset_;
           size_t index_custom_interface_with_following_offset_;
          };
          
          typedef GenericSystem GenericRobot;
          
          } // namespace mock_components
          
          #endif // MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_

ros2_control/hardware_interface/include/mock_components/visibility_control.h

       1  // Copyright (  c ) 2021 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef MOCK_COMPONENTS__VISIBILITY_CONTROL_H_
          #define MOCK_COMPONENTS__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define MOCK_COMPONENTS_EXPORT __attribute__(  (  dllexport ) )
          #define MOCK_COMPONENTS_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define MOCK_COMPONENTS_EXPORT __declspec(  dllexport )
          #define MOCK_COMPONENTS_IMPORT __declspec(  dllimport )
          #endif
          #ifdef MOCK_COMPONENTS_BUILDING_DLL
          #define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_EXPORT
          #else
          #define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_IMPORT
          #endif
          #define MOCK_COMPONENTS_PUBLIC_TYPE MOCK_COMPONENTS_PUBLIC
          #define MOCK_COMPONENTS_LOCAL
          #else
          #define MOCK_COMPONENTS_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define MOCK_COMPONENTS_IMPORT
          #if __GNUC__ >= 4
          #define MOCK_COMPONENTS_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define MOCK_COMPONENTS_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define MOCK_COMPONENTS_PUBLIC
          #define MOCK_COMPONENTS_LOCAL
          #endif
          #define MOCK_COMPONENTS_PUBLIC_TYPE
          #endif
          
          #endif // MOCK_COMPONENTS__VISIBILITY_CONTROL_H_

ros2_control/hardware_interface/src/actuator.cpp

       1  // Copyright 2020-2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "hardware_interface/actuator.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/actuator_interface.hpp"
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          // TODO(  destogl ): Add transition names also
          namespace hardware_interface
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      37  Actuator::Actuator(  std::unique_ptr<ActuatorInterface> impl ) : impl_(  std::move(  impl ) ) {}
          
      39  const rclcpp_lifecycle::State & Actuator::initialize(  const HardwareInfo & actuator_info )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_init(  actuator_info ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      60  const rclcpp_lifecycle::State & Actuator::configure(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
           {
           switch (  impl_->on_configure(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      83  const rclcpp_lifecycle::State & Actuator::cleanup(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_cleanup(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     103  const rclcpp_lifecycle::State & Actuator::shutdown(   )
          {
           if (  
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
           {
           switch (  impl_->on_shutdown(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     124  const rclcpp_lifecycle::State & Actuator::activate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_activate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     146  const rclcpp_lifecycle::State & Actuator::deactivate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           switch (  impl_->on_deactivate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     168  const rclcpp_lifecycle::State & Actuator::error(   )
          {
           if (  impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_error(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     189  std::vector<StateInterface> Actuator::export_state_interfaces(   )
          {
           // TODO(  karsten1987 ): Might be worth to do some brief sanity check here
           return impl_->export_state_interfaces(   );
          }
          
     195  std::vector<CommandInterface> Actuator::export_command_interfaces(   )
          {
           // TODO(  karsten1987 ): Might be worth to do some brief sanity check here
           return impl_->export_command_interfaces(   );
          }
          
     201  return_type Actuator::prepare_command_mode_switch(  
     202   const std::vector<std::string> & start_interfaces,  
     203   const std::vector<std::string> & stop_interfaces )
          {
           return impl_->prepare_command_mode_switch(  start_interfaces,   stop_interfaces );
          }
          
     208  return_type Actuator::perform_command_mode_switch(  
     209   const std::vector<std::string> & start_interfaces,  
     210   const std::vector<std::string> & stop_interfaces )
          {
           return impl_->perform_command_mode_switch(  start_interfaces,   stop_interfaces );
          }
          
     215  std::string Actuator::get_name(   ) const { return impl_->get_name(   ); }
          
     217  const rclcpp_lifecycle::State & Actuator::get_state(   ) const { return impl_->get_state(   ); }
          
     219  return_type Actuator::read(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type result = return_type::ERROR;
           if (  
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = impl_->read(  time,   period );
           if (  result == return_type::ERROR )
           {
           error(   );
           }
           }
           return result;
          }
          
     235  return_type Actuator::write(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type result = return_type::ERROR;
           if (  
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = impl_->write(  time,   period );
           if (  result == return_type::ERROR )
           {
           error(   );
           }
           }
           return result;
          }
          
          } // namespace hardware_interface

ros2_control/hardware_interface/src/component_parser.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <tinyxml2.h>
          #include <regex>
          #include <stdexcept>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          
          namespace
          {
      27  constexpr const auto kRobotTag = "robot";
      28  constexpr const auto kROS2ControlTag = "ros2_control";
      29  constexpr const auto kHardwareTag = "hardware";
      30  constexpr const auto kClassTypeTag = "plugin";
          constexpr const auto kParamTag = "param";
          constexpr const auto kActuatorTag = "actuator";
          constexpr const auto kJointTag = "joint";
          constexpr const auto kSensorTag = "sensor";
          constexpr const auto kGPIOTag = "gpio";
          constexpr const auto kTransmissionTag = "transmission";
          constexpr const auto kCommandInterfaceTag = "command_interface";
          constexpr const auto kStateInterfaceTag = "state_interface";
          constexpr const auto kMinTag = "min";
          constexpr const auto kMaxTag = "max";
          constexpr const auto kInitialValueTag = "initial_value";
          constexpr const auto kDataTypeAttribute = "data_type";
          constexpr const auto kSizeAttribute = "size";
          constexpr const auto kNameAttribute = "name";
          constexpr const auto kTypeAttribute = "type";
          constexpr const auto kRoleAttribute = "role";
          constexpr const auto kReductionAttribute = "mechanical_reduction";
          constexpr const auto kOffsetAttribute = "offset";
          } // namespace
          
          namespace hardware_interface
          {
          namespace detail
          {
          /// Gets value of the text between tags.
          /**
           * \param[in] element_it XMLElement iterator to search for the text.
           * \param[in] tag_name parent tag name where text is searched for (  used for error output )
           * \return text of for the tag
           * \throws std::runtime_error if text is not found
           */
          std::string get_text_for_element(  
           const tinyxml2::XMLElement * element_it,   const std::string & tag_name )
          {
           const auto get_text_output = element_it->GetText(   );
           if (  !get_text_output )
           {
           throw std::runtime_error(  "text not specified in the " + tag_name + " tag" );
           }
           return get_text_output;
          }
          
          /// Gets value of the attribute on an XMLelement.
          /**
           * If attribute is not found throws an error.
           *
           * \param[in] element_it XMLElement iterator to search for the attribute
           * \param[in] attribute_name attribute name to search for and return value
           * \param[in] tag_name parent tag name where attribute is searched for (  used for error output )
           * \return attribute value
           * \throws std::runtime_error if attribute is not found
           */
          std::string get_attribute_value(  
           const tinyxml2::XMLElement * element_it,   const char * attribute_name,   std::string tag_name )
          {
           const tinyxml2::XMLAttribute * attr;
           attr = element_it->FindAttribute(  attribute_name );
           if (  !attr )
           {
           throw std::runtime_error(  
           "no attribute " + std::string(  attribute_name ) + " in " + tag_name + " tag" );
           }
           return element_it->Attribute(  attribute_name );
          }
          
          /// Gets value of the attribute on an XMLelement.
          /**
           * If attribute is not found throws an error.
           *
           * \param[in] element_it XMLElement iterator to search for the attribute
           * \param[in] attribute_name attribute name to search for and return value
           * \param[in] tag_name parent tag name where attribute is searched for (  used for error output )
           * \return attribute value
           * \throws std::runtime_error if attribute is not found
           */
          std::string get_attribute_value(  
           const tinyxml2::XMLElement * element_it,   const char * attribute_name,   const char * tag_name )
          {
           return get_attribute_value(  element_it,   attribute_name,   std::string(  tag_name ) );
          }
          
          /// Gets value of the parameter on an XMLelement.
          /**
           * If parameter is not found,   returns specified default value
           *
           * \param[in] element_it XMLElement iterator to search for the attribute
           * \param[in] attribute_name attribute name to search for and return value
           * \param[in] default_value When the attribute is not found,   this value is returned instead
           * \return attribute value or default
           */
          double get_parameter_value_or(  
           const tinyxml2::XMLElement * params_it,   const char * parameter_name,   const double default_value )
          {
           while (  params_it )
           {
           try
           {
           // Fill the map with parameters
           const auto tag_name = params_it->Name(   );
           if (  strcmp(  tag_name,   parameter_name ) == 0 )
           {
           const auto tag_text = params_it->GetText(   );
           if (  tag_text )
           {
           return std::stod(  tag_text );
           }
           }
           }
           catch (  const std::exception & e )
           {
           return default_value;
           }
          
           params_it = params_it->NextSiblingElement(   );
           }
           return default_value;
          }
          
          /// Parse optional size attribute
          /**
           * Parses an XMLElement and returns the value of the size attribute.
           * If not specified,   defaults to 1. If not given a positive integer,   throws an error.
           *
           * \param[in] elem XMLElement that has the size attribute.
           * \return The size.
           * \throws std::runtime_error if not given a positive non-zero integer as value.
           */
          std::size_t parse_size_attribute(  const tinyxml2::XMLElement * elem )
          {
           const tinyxml2::XMLAttribute * attr = elem->FindAttribute(  kSizeAttribute );
          
           if (  !attr )
           {
           return 1;
           }
          
           std::size_t size;
           // Regex used to check for non-zero positive int
           std::string s = attr->Value(   );
           std::regex int_re(  "[1-9][0-9]*" );
           if (  std::regex_match(  s,   int_re ) )
           {
           size = std::stoi(  s );
           }
           else
           {
           throw std::runtime_error(  
           "Could not parse size tag in \"" + std::string(  elem->Name(   ) ) + "\"." + "Got \"" + s +
           "\",   but expected a non-zero positive integer." );
           }
          
           return size;
          }
          
          /// Parse data_type attribute
          /**
           * Parses an XMLElement and returns the value of the data_type attribute.
           * Defaults to "double" if not specified.
           *
           * \param[in] elem XMLElement that has the data_type attribute.
           * \return string specifying the data type.
           */
          std::string parse_data_type_attribute(  const tinyxml2::XMLElement * elem )
          {
           const tinyxml2::XMLAttribute * attr = elem->FindAttribute(  kDataTypeAttribute );
           std::string data_type;
           if (  !attr )
           {
           data_type = "double";
           }
           else
           {
           data_type = attr->Value(   );
           }
          
           return data_type;
          }
          
          /// Search XML snippet from URDF for parameters.
          /**
           * \param[in] params_it pointer to the iterator where parameters info should be found
           * \return key-value map with parameters
           * \throws std::runtime_error if a component attribute or tag is not found
           */
          std::unordered_map<std::string,   std::string> parse_parameters_from_xml(  
           const tinyxml2::XMLElement * params_it )
          {
           std::unordered_map<std::string,   std::string> parameters;
           const tinyxml2::XMLAttribute * attr;
          
           while (  params_it )
           {
           // Fill the map with parameters
           attr = params_it->FindAttribute(  kNameAttribute );
           if (  !attr )
           {
           throw std::runtime_error(  "no parameter name attribute set in param tag" );
           }
           const std::string parameter_name = params_it->Attribute(  kNameAttribute );
           const std::string parameter_value = get_text_for_element(  params_it,   parameter_name );
           parameters[parameter_name] = parameter_value;
          
           params_it = params_it->NextSiblingElement(  kParamTag );
           }
           return parameters;
          }
          
          /// Search XML snippet for definition of interfaceTypes.
          /**
           * \param[in] interfaces_it pointer to the iterator over interfaces
           * \param[in] interfaceTag interface type tag (  command or state )
           * \return list of interface types
           * \throws std::runtime_error if the interfaceType text not set in a tag
           */
          hardware_interface::InterfaceInfo parse_interfaces_from_xml(  
           const tinyxml2::XMLElement * interfaces_it )
          {
           hardware_interface::InterfaceInfo interface;
          
           const std::string interface_name =
           get_attribute_value(  interfaces_it,   kNameAttribute,   interfaces_it->Name(   ) );
           interface.name = interface_name;
          
           // Optional min/max attributes
           std::unordered_map<std::string,   std::string> interface_params =
           parse_parameters_from_xml(  interfaces_it->FirstChildElement(  kParamTag ) );
           auto interface_param = interface_params.find(  kMinTag );
           if (  interface_param != interface_params.end(   ) )
           {
           interface.min = interface_param->second;
           }
           interface_param = interface_params.find(  kMaxTag );
           if (  interface_param != interface_params.end(   ) )
           {
           interface.max = interface_param->second;
           }
          
           // Optional initial_value attribute
           interface_param = interface_params.find(  kInitialValueTag );
           if (  interface_param != interface_params.end(   ) )
           {
           interface.initial_value = interface_param->second;
           }
          
           // Default to a single double
           interface.data_type = "double";
           interface.size = 1;
          
           return interface;
          }
          
          /// Search XML snippet from URDF for information about a control component.
          /**
           * \param[in] component_it pointer to the iterator where component
           * info should be found
           * \return ComponentInfo filled with information about component
           * \throws std::runtime_error if a component attribute or tag is not found
           */
          ComponentInfo parse_component_from_xml(  const tinyxml2::XMLElement * component_it )
          {
           ComponentInfo component;
          
           // Find name,   type and class of a component
           component.type = component_it->Name(   );
           component.name = get_attribute_value(  component_it,   kNameAttribute,   component.type );
          
           // Parse all command interfaces
           const auto * command_interfaces_it = component_it->FirstChildElement(  kCommandInterfaceTag );
           while (  command_interfaces_it )
           {
           component.command_interfaces.push_back(  parse_interfaces_from_xml(  command_interfaces_it ) );
           command_interfaces_it = command_interfaces_it->NextSiblingElement(  kCommandInterfaceTag );
           }
          
           // Parse state interfaces
           const auto * state_interfaces_it = component_it->FirstChildElement(  kStateInterfaceTag );
           while (  state_interfaces_it )
           {
           component.state_interfaces.push_back(  parse_interfaces_from_xml(  state_interfaces_it ) );
           state_interfaces_it = state_interfaces_it->NextSiblingElement(  kStateInterfaceTag );
           }
          
           // Parse parameters
           const auto * params_it = component_it->FirstChildElement(  kParamTag );
           if (  params_it )
           {
           component.parameters = parse_parameters_from_xml(  params_it );
           }
          
           return component;
          }
          
          /// Search XML snippet from URDF for information about a complex component.
          /**
           * A complex component can have a non-double data type specified on its interfaces,  
           * and the interface may be an array of a fixed size of the data type.
           *
           * \param[in] component_it pointer to the iterator where component
           * info should befound
           * \throws std::runtime_error if a required component attribute or tag is not found.
           */
          ComponentInfo parse_complex_component_from_xml(  const tinyxml2::XMLElement * component_it )
          {
           ComponentInfo component;
          
           // Find name,   type and class of a component
           component.type = component_it->Name(   );
           component.name = get_attribute_value(  component_it,   kNameAttribute,   component.type );
          
           // Parse all command interfaces
           const auto * command_interfaces_it = component_it->FirstChildElement(  kCommandInterfaceTag );
           while (  command_interfaces_it )
           {
           component.command_interfaces.push_back(  parse_interfaces_from_xml(  command_interfaces_it ) );
           component.command_interfaces.back(   ).data_type =
           parse_data_type_attribute(  command_interfaces_it );
           component.command_interfaces.back(   ).size = parse_size_attribute(  command_interfaces_it );
           command_interfaces_it = command_interfaces_it->NextSiblingElement(  kCommandInterfaceTag );
           }
          
           // Parse state interfaces
           const auto * state_interfaces_it = component_it->FirstChildElement(  kStateInterfaceTag );
           while (  state_interfaces_it )
           {
           component.state_interfaces.push_back(  parse_interfaces_from_xml(  state_interfaces_it ) );
           component.state_interfaces.back(   ).data_type = parse_data_type_attribute(  state_interfaces_it );
           component.state_interfaces.back(   ).size = parse_size_attribute(  state_interfaces_it );
           state_interfaces_it = state_interfaces_it->NextSiblingElement(  kStateInterfaceTag );
           }
          
           // Parse parameters
           const auto * params_it = component_it->FirstChildElement(  kParamTag );
           if (  params_it )
           {
           component.parameters = parse_parameters_from_xml(  params_it );
           }
          
           return component;
          }
          
          JointInfo parse_transmission_joint_from_xml(  const tinyxml2::XMLElement * element_it )
          {
           JointInfo joint_info;
           joint_info.name = get_attribute_value(  element_it,   kNameAttribute,   element_it->Name(   ) );
           joint_info.role = get_attribute_value(  element_it,   kRoleAttribute,   element_it->Name(   ) );
           joint_info.mechanical_reduction =
           get_parameter_value_or(  element_it->FirstChildElement(   ),   kReductionAttribute,   1.0 );
           joint_info.offset =
           get_parameter_value_or(  element_it->FirstChildElement(   ),   kOffsetAttribute,   0.0 );
           return joint_info;
          }
          
          ActuatorInfo parse_transmission_actuator_from_xml(  const tinyxml2::XMLElement * element_it )
          {
           ActuatorInfo actuator_info;
           actuator_info.name = get_attribute_value(  element_it,   kNameAttribute,   element_it->Name(   ) );
           actuator_info.role = get_attribute_value(  element_it,   kRoleAttribute,   element_it->Name(   ) );
           actuator_info.mechanical_reduction =
           get_parameter_value_or(  element_it->FirstChildElement(   ),   kReductionAttribute,   1.0 );
           actuator_info.offset =
           get_parameter_value_or(  element_it->FirstChildElement(   ),   kOffsetAttribute,   0.0 );
           return actuator_info;
          }
          
          /// Search XML snippet from URDF for information about a transmission.
          /**
           * \param[in] transmission_it pointer to the iterator where transmission info should be found
           * \return TransmissionInfo filled with information about transmission
           * \throws std::runtime_error if an attribute or tag is not found
           */
          TransmissionInfo parse_transmission_from_xml(  const tinyxml2::XMLElement * transmission_it )
          {
           TransmissionInfo transmission;
          
           // Find name,   type and class of a transmission
           transmission.name = get_attribute_value(  transmission_it,   kNameAttribute,   transmission_it->Name(   ) );
           const auto * type_it = transmission_it->FirstChildElement(  kClassTypeTag );
           transmission.type = get_text_for_element(  type_it,   kClassTypeTag );
          
           // Parse joints
           const auto * joint_it = transmission_it->FirstChildElement(  kJointTag );
           while (  joint_it )
           {
           transmission.joints.push_back(  parse_transmission_joint_from_xml(  joint_it ) );
           joint_it = joint_it->NextSiblingElement(  kJointTag );
           }
          
           // Parse actuators
           const auto * actuator_it = transmission_it->FirstChildElement(  kActuatorTag );
           while (  actuator_it )
           {
           transmission.actuators.push_back(  parse_transmission_actuator_from_xml(  actuator_it ) );
           actuator_it = actuator_it->NextSiblingElement(  kActuatorTag );
           }
          
           // Parse parameters
           const auto * params_it = transmission_it->FirstChildElement(  kParamTag );
           if (  params_it )
           {
           transmission.parameters = parse_parameters_from_xml(  params_it );
           }
          
           return transmission;
          }
          
          /// Auto-fill some contents of transmission info based on context
          /**
           * \param[in,  out] hardware HardwareInfo structure with elements already parsed.
           * \throws std::runtime_error
           */
          void auto_fill_transmission_interfaces(  HardwareInfo & hardware )
          {
           for (  auto & transmission : hardware.transmissions )
           {
           // fill joint interfaces for actuator from joints declared for this component
           for (  auto & joint : transmission.joints )
           {
           auto found_it = std::find_if(  
           hardware.joints.cbegin(   ),   hardware.joints.cend(   ),  
           [&joint](  const auto & joint_info ) { return joint.name == joint_info.name; } );
          
           if (  found_it == hardware.joints.cend(   ) )
           {
           throw std::runtime_error(  
           "Error while parsing '" + hardware.name + "'. Transmission '" + transmission.name +
           "' declared joint '" + joint.name + "' is not available in component '" + hardware.name +
           "'." );
           }
          
           // copy interface names from their definitions in the component
           std::transform(  
           found_it->command_interfaces.cbegin(   ),   found_it->command_interfaces.cend(   ),  
           std::back_inserter(  joint.interfaces ),  
           [](  const auto & interface ) { return interface.name; } );
           }
          
           // we parsed an actuator component,   here we fill in more details
           if (  hardware.type == kActuatorTag )
           {
           if (  transmission.joints.size(   ) != 1 )
           {
           throw std::runtime_error(  
           "Error while parsing '" + hardware.name +
           "'. There should be exactly one joint defined in this component but found " +
           std::to_string(  transmission.joints.size(   ) ) );
           }
          
           transmission.actuators.push_back(  
           ActuatorInfo{"actuator1",   transmission.joints[0].interfaces,   "actuator1",   1.0,   0.0} );
           }
           }
          }
          
          /// Parse a control resource from an "ros2_control" tag.
          /**
           * \param[in] ros2_control_it pointer to ros2_control element
           * with information about resource.
           * \return HardwareInfo filled with information about the robot
           * \throws std::runtime_error if a attributes or tag are not found
           */
          HardwareInfo parse_resource_from_xml(  
           const tinyxml2::XMLElement * ros2_control_it,   const std::string & urdf )
          {
           HardwareInfo hardware;
           hardware.name = get_attribute_value(  ros2_control_it,   kNameAttribute,   kROS2ControlTag );
           hardware.type = get_attribute_value(  ros2_control_it,   kTypeAttribute,   kROS2ControlTag );
          
           // Parse everything under ros2_control tag
           hardware.hardware_class_type = "";
           const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(   );
           while (  ros2_control_child_it )
           {
           if (  !std::string(  kHardwareTag ).compare(  ros2_control_child_it->Name(   ) ) )
           {
           const auto * type_it = ros2_control_child_it->FirstChildElement(  kClassTypeTag );
           hardware.hardware_class_type =
           get_text_for_element(  type_it,   std::string(  "hardware " ) + kClassTypeTag );
           const auto * params_it = ros2_control_child_it->FirstChildElement(  kParamTag );
           if (  params_it )
           {
           hardware.hardware_parameters = parse_parameters_from_xml(  params_it );
           }
           }
           else if (  !std::string(  kJointTag ).compare(  ros2_control_child_it->Name(   ) ) )
           {
           hardware.joints.push_back(  parse_component_from_xml(  ros2_control_child_it ) );
           }
           else if (  !std::string(  kSensorTag ).compare(  ros2_control_child_it->Name(   ) ) )
           {
           hardware.sensors.push_back(  parse_component_from_xml(  ros2_control_child_it ) );
           }
           else if (  !std::string(  kGPIOTag ).compare(  ros2_control_child_it->Name(   ) ) )
           {
           hardware.gpios.push_back(  parse_complex_component_from_xml(  ros2_control_child_it ) );
           }
           else if (  !std::string(  kTransmissionTag ).compare(  ros2_control_child_it->Name(   ) ) )
           {
           hardware.transmissions.push_back(  parse_transmission_from_xml(  ros2_control_child_it ) );
           }
           else
           {
           throw std::runtime_error(  "invalid tag name " + std::string(  ros2_control_child_it->Name(   ) ) );
           }
           ros2_control_child_it = ros2_control_child_it->NextSiblingElement(   );
           }
          
           auto_fill_transmission_interfaces(  hardware );
          
           hardware.original_xml = urdf;
          
           return hardware;
          }
          
          } // namespace detail
          
          std::vector<HardwareInfo> parse_control_resources_from_urdf(  const std::string & urdf )
          {
           // Check if everything OK with URDF string
           if (  urdf.empty(   ) )
           {
           throw std::runtime_error(  "empty URDF passed to robot" );
           }
           tinyxml2::XMLDocument doc;
           if (  !doc.Parse(  urdf.c_str(   ) ) && doc.Error(   ) )
           {
           throw std::runtime_error(  "invalid URDF passed in to robot parser" );
           }
           if (  doc.Error(   ) )
           {
           throw std::runtime_error(  "invalid URDF passed in to robot parser" );
           }
          
           // Find robot tag
           const tinyxml2::XMLElement * robot_it = doc.RootElement(   );
          
           if (  std::string(  kRobotTag ).compare(  robot_it->Name(   ) ) )
           {
           throw std::runtime_error(  "the robot tag is not root element in URDF" );
           }
          
           const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(  kROS2ControlTag );
           if (  !ros2_control_it )
           {
           throw std::runtime_error(  "no " + std::string(  kROS2ControlTag ) + " tag" );
           }
          
           std::vector<HardwareInfo> hardware_info;
           while (  ros2_control_it )
           {
           hardware_info.push_back(  detail::parse_resource_from_xml(  ros2_control_it,   urdf ) );
           ros2_control_it = ros2_control_it->NextSiblingElement(  kROS2ControlTag );
           }
          
           return hardware_info;
          }
          
          } // namespace hardware_interface

ros2_control/hardware_interface/src/mock_components/fake_generic_system.cpp

       1  // Copyright (  c ) 2022 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Author: Jafar Abdi,   Denis Stogl
          
          #include "fake_components/generic_system.hpp"
          #include "pluginlib/class_list_macros.hpp"
      19  PLUGINLIB_EXPORT_CLASS(  fake_components::GenericSystem,   hardware_interface::SystemInterface )

ros2_control/hardware_interface/src/mock_components/generic_system.cpp

       1  // Copyright (  c ) 2021 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Author: Jafar Abdi,   Denis Stogl
          
          #include "mock_components/generic_system.hpp"
          
          #include <algorithm>
          #include <cmath>
          #include <iterator>
          #include <limits>
          #include <set>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "rcutils/logging_macros.h"
          
          namespace mock_components
          {
      32  CallbackReturn GenericSystem::on_init(  const hardware_interface::HardwareInfo & info )
          {
           if (  hardware_interface::SystemInterface::on_init(  info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           auto populate_non_standard_interfaces =
           [this](  auto interface_list,   auto & non_standard_interfaces )
           {
           for (  const auto & interface : interface_list )
           {
           // add to list if non-standard interface
           if (  
           std::find(  standard_interfaces_.begin(   ),   standard_interfaces_.end(   ),   interface.name ) ==
           standard_interfaces_.end(   ) )
           {
           if (  
           std::find(  
           non_standard_interfaces.begin(   ),   non_standard_interfaces.end(   ),   interface.name ) ==
           non_standard_interfaces.end(   ) )
           {
           non_standard_interfaces.emplace_back(  interface.name );
           }
           }
           }
           };
          
           // check if to create fake command interface for sensor
           auto it = info_.hardware_parameters.find(  "fake_sensor_commands" );
           if (  it != info_.hardware_parameters.end(   ) )
           {
           // TODO(  anyone ): change this to parse_bool(   ) (  see ros2_control#339 )
           use_fake_sensor_command_interfaces_ = it->second == "true" || it->second == "True";
           }
           else
           {
           use_fake_sensor_command_interfaces_ = false;
           }
          
           // check if to create fake command interface for gpio
           it = info_.hardware_parameters.find(  "fake_gpio_commands" );
           if (  it != info_.hardware_parameters.end(   ) )
           {
           // TODO(  anyone ): change this to parse_bool(   ) (  see ros2_control#339 )
           use_fake_gpio_command_interfaces_ = it->second == "true" || it->second == "True";
           }
           else
           {
           use_fake_gpio_command_interfaces_ = false;
           }
          
           // process parameters about state following
           position_state_following_offset_ = 0.0;
           custom_interface_with_following_offset_ = "";
          
           it = info_.hardware_parameters.find(  "position_state_following_offset" );
           if (  it != info_.hardware_parameters.end(   ) )
           {
           position_state_following_offset_ = std::stod(  it->second );
           it = info_.hardware_parameters.find(  "custom_interface_with_following_offset" );
           if (  it != info_.hardware_parameters.end(   ) )
           {
           custom_interface_with_following_offset_ = it->second;
           }
           }
           // its extremlly unprobably that std::distance results int this value - therefore default
           index_custom_interface_with_following_offset_ = std::numeric_limits<size_t>::max(   );
          
           // Initialize storage for standard interfaces
           initialize_storage_vectors(  joint_commands_,   joint_states_,   standard_interfaces_ );
           // set all values without initial values to 0
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           for (  auto j = 0u; j < standard_interfaces_.size(   ); j++ )
           {
           if (  std::isnan(  joint_states_[j][i] ) )
           {
           joint_states_[j][i] = 0.0;
           }
           }
           }
          
           // Search for mimic joints
           for (  auto i = 0u; i < info_.joints.size(   ); ++i )
           {
           const auto & joint = info_.joints.at(  i );
           if (  joint.parameters.find(  "mimic" ) != joint.parameters.cend(   ) )
           {
           const auto mimicked_joint_it = std::find_if(  
           info_.joints.begin(   ),   info_.joints.end(   ),  
           [&mimicked_joint =
           joint.parameters.at(  "mimic" )](  const hardware_interface::ComponentInfo & joint_info )
           { return joint_info.name == mimicked_joint; } );
           if (  mimicked_joint_it == info_.joints.cend(   ) )
           {
           throw std::runtime_error(  
           std::string(  "Mimicked joint '" ) + joint.parameters.at(  "mimic" ) + "' not found" );
           }
           MimicJoint mimic_joint;
           mimic_joint.joint_index = i;
           mimic_joint.mimicked_joint_index = std::distance(  info_.joints.begin(   ),   mimicked_joint_it );
           auto param_it = joint.parameters.find(  "multiplier" );
           if (  param_it != joint.parameters.end(   ) )
           {
           mimic_joint.multiplier = std::stod(  joint.parameters.at(  "multiplier" ) );
           }
           mimic_joints_.push_back(  mimic_joint );
           }
           }
          
           // search for non-standard joint interfaces
           for (  const auto & joint : info_.joints )
           {
           // populate non-standard command interfaces to other_interfaces_
           populate_non_standard_interfaces(  joint.command_interfaces,   other_interfaces_ );
          
           // populate non-standard state interfaces to other_interfaces_
           populate_non_standard_interfaces(  joint.state_interfaces,   other_interfaces_ );
           }
          
           // Initialize storage for non-standard interfaces
           initialize_storage_vectors(  other_commands_,   other_states_,   other_interfaces_ );
          
           // when following offset is used on custom interface then find its index
           if (  !custom_interface_with_following_offset_.empty(   ) )
           {
           auto if_it = std::find(  
           other_interfaces_.begin(   ),   other_interfaces_.end(   ),   custom_interface_with_following_offset_ );
           if (  if_it != other_interfaces_.end(   ) )
           {
           index_custom_interface_with_following_offset_ =
           std::distance(  other_interfaces_.begin(   ),   if_it );
           RCUTILS_LOG_INFO_NAMED(  
           "fake_generic_system",   "Custom interface with following offset '%s' found at index: %zu.",  
           custom_interface_with_following_offset_.c_str(   ),  
           index_custom_interface_with_following_offset_ );
           }
           else
           {
           RCUTILS_LOG_WARN_NAMED(  
           "fake_generic_system",  
           "Custom interface with following offset '%s' does not exist. Offset will not be applied",  
           custom_interface_with_following_offset_.c_str(   ) );
           }
           }
          
           for (  const auto & sensor : info_.sensors )
           {
           for (  const auto & interface : sensor.state_interfaces )
           {
           if (  
           std::find(  sensor_interfaces_.begin(   ),   sensor_interfaces_.end(   ),   interface.name ) ==
           sensor_interfaces_.end(   ) )
           {
           sensor_interfaces_.emplace_back(  interface.name );
           }
           }
           }
           initialize_storage_vectors(  sensor_fake_commands_,   sensor_states_,   sensor_interfaces_ );
          
           // search for gpio interfaces
           for (  const auto & gpio : info_.gpios )
           {
           // populate non-standard command interfaces to gpio_interfaces_
           populate_non_standard_interfaces(  gpio.command_interfaces,   gpio_interfaces_ );
          
           // populate non-standard state interfaces to gpio_interfaces_
           populate_non_standard_interfaces(  gpio.state_interfaces,   gpio_interfaces_ );
           }
          
           // Fake gpio command interfaces
           if (  use_fake_gpio_command_interfaces_ )
           {
           initialize_storage_vectors(  gpio_fake_commands_,   gpio_states_,   gpio_interfaces_ );
           }
           // Real gpio command interfaces
           else
           {
           initialize_storage_vectors(  gpio_commands_,   gpio_states_,   gpio_interfaces_ );
           }
          
           return CallbackReturn::SUCCESS;
          }
          
     217  std::vector<hardware_interface::StateInterface> GenericSystem::export_state_interfaces(   )
          {
           std::vector<hardware_interface::StateInterface> state_interfaces;
          
           // Joints' state interfaces
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           const auto & joint = info_.joints[i];
           for (  const auto & interface : joint.state_interfaces )
           {
           // Add interface: if not in the standard list then use "other" interface list
           if (  !get_interface(  
           joint.name,   standard_interfaces_,   interface.name,   i,   joint_states_,   state_interfaces ) )
           {
           if (  !get_interface(  
           joint.name,   other_interfaces_,   interface.name,   i,   other_states_,   state_interfaces ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the standard nor other list. "
           "This should never happen!" );
           }
           }
           }
           }
          
           // Sensor state interfaces
           if (  !populate_interfaces(  
           info_.sensors,   sensor_interfaces_,   sensor_states_,   state_interfaces,   true ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the standard nor other list. This should never happen!" );
           };
          
           // GPIO state interfaces
           if (  !populate_interfaces(  info_.gpios,   gpio_interfaces_,   gpio_states_,   state_interfaces,   true ) )
           {
           throw std::runtime_error(  "Interface is not found in the gpio list. This should never happen!" );
           }
          
           return state_interfaces;
          }
          
     259  std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_interfaces(   )
          {
           std::vector<hardware_interface::CommandInterface> command_interfaces;
          
           // Joints' state interfaces
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           const auto & joint = info_.joints[i];
           for (  const auto & interface : joint.command_interfaces )
           {
           // Add interface: if not in the standard list than use "other" interface list
           if (  !get_interface(  
           joint.name,   standard_interfaces_,   interface.name,   i,   joint_commands_,  
           command_interfaces ) )
           {
           if (  !get_interface(  
           joint.name,   other_interfaces_,   interface.name,   i,   other_commands_,  
           command_interfaces ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the standard nor other list. "
           "This should never happen!" );
           }
           }
           }
           }
          
           // Fake sensor command interfaces
           if (  use_fake_sensor_command_interfaces_ )
           {
           if (  !populate_interfaces(  
           info_.sensors,   sensor_interfaces_,   sensor_fake_commands_,   command_interfaces,   true ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the standard nor other list. This should never happen!" );
           }
           }
          
           // Fake gpio command interfaces (  consider all state interfaces for command interfaces )
           if (  use_fake_gpio_command_interfaces_ )
           {
           if (  !populate_interfaces(  
           info_.gpios,   gpio_interfaces_,   gpio_fake_commands_,   command_interfaces,   true ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the gpio list. This should never happen!" );
           }
           }
           // GPIO command interfaces (  real command interfaces )
           else
           {
           if (  !populate_interfaces(  
           info_.gpios,   gpio_interfaces_,   gpio_commands_,   command_interfaces,   false ) )
           {
           throw std::runtime_error(  
           "Interface is not found in the gpio list. This should never happen!" );
           }
           }
          
           return command_interfaces;
          }
          
     321  return_type GenericSystem::read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           auto mirror_command_to_state = [](  auto & states_,   auto commands_,   size_t start_index = 0 )
           {
           for (  size_t i = start_index; i < states_.size(   ); ++i )
           {
           for (  size_t j = 0; j < states_[i].size(   ); ++j )
           {
           if (  !std::isnan(  commands_[i][j] ) )
           {
           states_[i][j] = commands_[i][j];
           }
           }
           }
           };
          
           // apply offset to positions only
           for (  size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(   ); ++j )
           {
           if (  !std::isnan(  joint_commands_[POSITION_INTERFACE_INDEX][j] ) )
           {
           joint_states_[POSITION_INTERFACE_INDEX][j] =
           joint_commands_[POSITION_INTERFACE_INDEX][j] +
           (  custom_interface_with_following_offset_.empty(   ) ? position_state_following_offset_ : 0.0 );
           }
           }
          
           // do loopback on all other interfaces - starts from 1 because 0 index is position interface
           mirror_command_to_state(  joint_states_,   joint_commands_,   1 );
          
           for (  const auto & mimic_joint : mimic_joints_ )
           {
           for (  auto i = 0u; i < joint_states_.size(   ); ++i )
           {
           joint_states_[i][mimic_joint.joint_index] =
           mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index];
           }
           }
          
           for (  size_t i = 0; i < other_states_.size(   ); ++i )
           {
           for (  size_t j = 0; j < other_states_[i].size(   ); ++j )
           {
           if (  
           i == index_custom_interface_with_following_offset_ &&
           !std::isnan(  joint_commands_[POSITION_INTERFACE_INDEX][j] ) )
           {
           other_states_[i][j] =
           joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_;
           }
           else if (  !std::isnan(  other_commands_[i][j] ) )
           {
           other_states_[i][j] = other_commands_[i][j];
           }
           }
           }
          
           if (  use_fake_sensor_command_interfaces_ )
           {
           mirror_command_to_state(  sensor_states_,   sensor_fake_commands_ );
           }
          
           // do loopback on all gpio interfaces
           if (  use_fake_gpio_command_interfaces_ )
           {
           mirror_command_to_state(  gpio_states_,   gpio_fake_commands_ );
           }
           else
           {
           mirror_command_to_state(  gpio_states_,   gpio_commands_ );
           }
          
           return return_type::OK;
          }
          
          // Private methods
          template <typename HandleType>
     398  bool GenericSystem::get_interface(  
           const std::string & name,   const std::vector<std::string> & interface_list,  
           const std::string & interface_name,   const size_t vector_index,  
           std::vector<std::vector<double>> & values,   std::vector<HandleType> & interfaces )
          {
           auto it = std::find(  interface_list.begin(   ),   interface_list.end(   ),   interface_name );
           if (  it != interface_list.end(   ) )
           {
           auto j = std::distance(  interface_list.begin(   ),   it );
           interfaces.emplace_back(  name,   *it,   &values[j][vector_index] );
           return true;
           }
           return false;
          }
          
     413  void GenericSystem::initialize_storage_vectors(  
           std::vector<std::vector<double>> & commands,   std::vector<std::vector<double>> & states,  
           const std::vector<std::string> & interfaces )
          {
           // Initialize storage for all joints,   regardless of their existence
           commands.resize(  interfaces.size(   ) );
           states.resize(  interfaces.size(   ) );
           for (  auto i = 0u; i < interfaces.size(   ); i++ )
           {
           commands[i].resize(  info_.joints.size(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           states[i].resize(  info_.joints.size(   ),   std::numeric_limits<double>::quiet_NaN(   ) );
           }
          
           // Initialize with values from URDF
           bool print_hint = false;
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           const auto & joint = info_.joints[i];
           for (  const auto & interface : joint.state_interfaces )
           {
           auto it = std::find(  interfaces.begin(   ),   interfaces.end(   ),   interface.name );
          
           // If interface name is found in the interfaces list
           if (  it != interfaces.end(   ) )
           {
           auto index = std::distance(  interfaces.begin(   ),   it );
          
           // Check the initial_value param is used
           if (  !interface.initial_value.empty(   ) )
           {
           states[index][i] = std::stod(  interface.initial_value );
           }
           else
           {
           // Initialize the value in old way with warning message
           auto it2 = joint.parameters.find(  "initial_" + interface.name );
           if (  it2 != joint.parameters.end(   ) )
           {
           states[index][i] = std::stod(  it2->second );
           print_hint = true;
           }
           else
           {
           print_hint = true;
           }
           }
           }
           }
           }
           if (  print_hint )
           {
           RCUTILS_LOG_WARN_ONCE_NAMED(  
           "mock_generic_system",  
           "Parsing of optional initial interface values failed or uses a deprecated format. Add "
           "initial values for every state interface in the ros2_control.xacro. For example: \n"
           "<state_interface name=\"velocity\"> \n"
           " <param name=\"initial_value\">0.0</param> \n"
           "</state_interface>" );
           }
          }
          
          template <typename InterfaceType>
     475  bool GenericSystem::populate_interfaces(  
           const std::vector<hardware_interface::ComponentInfo> & components,  
           std::vector<std::string> & interface_names,   std::vector<std::vector<double>> & storage,  
           std::vector<InterfaceType> & target_interfaces,   bool using_state_interfaces )
          {
           for (  auto i = 0u; i < components.size(   ); i++ )
           {
           const auto & component = components[i];
           const auto interfaces =
           (  using_state_interfaces ) ? component.state_interfaces : component.command_interfaces;
           for (  const auto & interface : interfaces )
           {
           if (  !get_interface(  
           component.name,   interface_names,   interface.name,   i,   storage,   target_interfaces ) )
           {
           return false;
           }
           }
           }
          
           return true;
          }
          } // namespace mock_components
          
          #include "pluginlib/class_list_macros.hpp"
     500  PLUGINLIB_EXPORT_CLASS(  mock_components::GenericSystem,   hardware_interface::SystemInterface )

ros2_control/hardware_interface/src/resource_manager.cpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "hardware_interface/resource_manager.hpp"
          
          #include <functional>
          #include <map>
          #include <memory>
          #include <string>
          #include <unordered_map>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/actuator.hpp"
          #include "hardware_interface/actuator_interface.hpp"
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_component_info.hpp"
          #include "hardware_interface/sensor.hpp"
          #include "hardware_interface/sensor_interface.hpp"
          #include "hardware_interface/system.hpp"
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "rcutils/logging_macros.h"
          
          namespace hardware_interface
          {
      40  auto trigger_and_print_hardware_state_transition =
           [](  
           auto transition,   const std::string transition_name,   const std::string & hardware_name,  
           const lifecycle_msgs::msg::State::_id_type & target_state )
          {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "'%s' hardware '%s' ",   transition_name.c_str(   ),   hardware_name.c_str(   ) );
          
           const rclcpp_lifecycle::State new_state = transition(   );
          
           bool result = new_state.id(   ) == target_state;
          
           if (  result )
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Successful '%s' of hardware '%s'",   transition_name.c_str(   ),  
           hardware_name.c_str(   ) );
           }
           else
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Failed to '%s' hardware '%s'",   transition_name.c_str(   ),  
           hardware_name.c_str(   ) );
           }
           return result;
          };
          
      67  class ResourceStorage
          {
           static constexpr const char * pkg_name = "hardware_interface";
          
      71   static constexpr const char * actuator_interface_name = "hardware_interface::ActuatorInterface";
      72   static constexpr const char * sensor_interface_name = "hardware_interface::SensorInterface";
      73   static constexpr const char * system_interface_name = "hardware_interface::SystemInterface";
          
          public:
           ResourceStorage(   )
           : actuator_loader_(  pkg_name,   actuator_interface_name ),  
           sensor_loader_(  pkg_name,   sensor_interface_name ),  
           system_loader_(  pkg_name,   system_interface_name )
           {
           }
          
           template <class HardwareT,   class HardwareInterfaceT>
           void load_hardware(  
           const HardwareInfo & hardware_info,   pluginlib::ClassLoader<HardwareInterfaceT> & loader,  
           std::vector<HardwareT> & container )
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Loading hardware '%s' ",   hardware_info.name.c_str(   ) );
           // hardware_class_type has to match class name in plugin xml description
           // TODO(  karsten1987 ) extract package from hardware_class_type
           // e.g.: <package_vendor>/<system_type>
           auto interface = std::unique_ptr<HardwareInterfaceT>(  
           loader.createUnmanagedInstance(  hardware_info.hardware_class_type ) );
           HardwareT hardware(  std::move(  interface ) );
           container.emplace_back(  std::move(  hardware ) );
           // initialize static data about hardware component to reduce later calls
           HardwareComponentInfo component_info;
           component_info.name = hardware_info.name;
           component_info.type = hardware_info.type;
           component_info.class_type = hardware_info.hardware_class_type;
          
           hardware_info_map_.insert(  std::make_pair(  component_info.name,   component_info ) );
           }
          
           template <class HardwareT>
           bool initialize_hardware(  const HardwareInfo & hardware_info,   HardwareT & hardware )
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Initialize hardware '%s' ",   hardware_info.name.c_str(   ) );
          
           const rclcpp_lifecycle::State new_state = hardware.initialize(  hardware_info );
          
           bool result = new_state.id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
          
           if (  result )
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Successful initialization of hardware '%s'",  
           hardware_info.name.c_str(   ) );
           }
           else
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Failed to initialize hardware '%s'",   hardware_info.name.c_str(   ) );
           }
           return result;
           }
          
           template <class HardwareT>
           bool configure_hardware(  HardwareT & hardware )
           {
           bool result = trigger_and_print_hardware_state_transition(  
           std::bind(  &HardwareT::configure,   &hardware ),   "configure",   hardware.get_name(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           if (  result )
           {
           // TODO(  destogl ): is it better to check here if previous state was unconfigured instead of
           // checking if each state already exists? Or we should somehow know that transition has
           // happened and only then trigger this part of the code?
           // On the other side this part of the code should never be executed in real-time critical
           // thread,   so it could be also OK as it is...
           for (  const auto & interface : hardware_info_map_[hardware.get_name(   )].state_interfaces )
           {
           // add all state interfaces to available list
           auto found_it = std::find(  
           available_state_interfaces_.begin(   ),   available_state_interfaces_.end(   ),   interface );
          
           if (  found_it == available_state_interfaces_.end(   ) )
           {
           available_state_interfaces_.emplace_back(  interface );
           RCUTILS_LOG_DEBUG_NAMED(  
           "resource_manager",   "(  hardware '%s' ): '%s' state interface added into available list",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           else
           {
           // TODO(  destogl ): do here error management if interfaces are only partially added into
           // "available" list - this should never be the case!
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",  
           "(  hardware '%s' ): '%s' state interface already in available list."
           " This can happen due to multiple calls to 'configure'",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           }
          
           // add command interfaces to available list
           for (  const auto & interface : hardware_info_map_[hardware.get_name(   )].command_interfaces )
           {
           // TODO(  destogl ): check if interface should be available on configure
           auto found_it = std::find(  
           available_command_interfaces_.begin(   ),   available_command_interfaces_.end(   ),   interface );
          
           if (  found_it == available_command_interfaces_.end(   ) )
           {
           available_command_interfaces_.emplace_back(  interface );
           RCUTILS_LOG_DEBUG_NAMED(  
           "resource_manager",   "(  hardware '%s' ): '%s' command interface added into available list",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           else
           {
           // TODO(  destogl ): do here error management if interfaces are only partially added into
           // "available" list - this should never be the case!
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",  
           "(  hardware '%s' ): '%s' command interface already in available list."
           " This can happen due to multiple calls to 'configure'",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           }
           }
           return result;
           }
          
           template <class HardwareT>
           bool cleanup_hardware(  HardwareT & hardware )
           {
           bool result = trigger_and_print_hardware_state_transition(  
           std::bind(  &HardwareT::cleanup,   &hardware ),   "cleanup",   hardware.get_name(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
          
           if (  result )
           {
           // remove all command interfaces from available list
           for (  const auto & interface : hardware_info_map_[hardware.get_name(   )].command_interfaces )
           {
           auto found_it = std::find(  
           available_command_interfaces_.begin(   ),   available_command_interfaces_.end(   ),   interface );
          
           if (  found_it != available_command_interfaces_.end(   ) )
           {
           available_command_interfaces_.erase(  found_it );
           RCUTILS_LOG_DEBUG_NAMED(  
           "resource_manager",  
           "(  hardware '%s' ): '%s' command interface removed from available list",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           else
           {
           // TODO(  destogl ): do here error management if interfaces are only partially added into
           // "available" list - this should never be the case!
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",  
           "(  hardware '%s' ): '%s' command interface not in available list."
           " This can happen due to multiple calls to 'cleanup'",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           }
           // remove all state interfaces from available list
           for (  const auto & interface : hardware_info_map_[hardware.get_name(   )].state_interfaces )
           {
           auto found_it = std::find(  
           available_state_interfaces_.begin(   ),   available_state_interfaces_.end(   ),   interface );
          
           if (  found_it != available_state_interfaces_.end(   ) )
           {
           available_state_interfaces_.erase(  found_it );
           RCUTILS_LOG_DEBUG_NAMED(  
           "resource_manager",   "(  hardware '%s' ): '%s' state interface removed from available list",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           else
           {
           // TODO(  destogl ): do here error management if interfaces are only partially added into
           // "available" list - this should never be the case!
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",  
           "(  hardware '%s' ): '%s' state interface not in available list. "
           "This can happen due to multiple calls to 'cleanup'",  
           hardware.get_name(   ).c_str(   ),   interface.c_str(   ) );
           }
           }
           }
           return result;
           }
          
           template <class HardwareT>
           bool shutdown_hardware(  HardwareT & hardware )
           {
           bool result = trigger_and_print_hardware_state_transition(  
           std::bind(  &HardwareT::shutdown,   &hardware ),   "shutdown",   hardware.get_name(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
          
           if (  result )
           {
           // TODO(  destogl ): change this - deimport all things if there is there are interfaces there
           // deimport_non_movement_command_interfaces(  hardware );
           // deimport_state_interfaces(  hardware );
           // use remove_command_interfaces(  hardware );
           }
           return result;
           }
          
           template <class HardwareT>
           bool activate_hardware(  HardwareT & hardware )
           {
           bool result = trigger_and_print_hardware_state_transition(  
           std::bind(  &HardwareT::activate,   &hardware ),   "activate",   hardware.get_name(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           if (  result )
           {
           // TODO(  destogl ): make all command interfaces available (  currently are all available )
           }
          
           return result;
           }
          
           template <class HardwareT>
           bool deactivate_hardware(  HardwareT & hardware )
           {
           bool result = trigger_and_print_hardware_state_transition(  
           std::bind(  &HardwareT::deactivate,   &hardware ),   "deactivate",   hardware.get_name(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           if (  result )
           {
           // TODO(  destogl ): make all command interfaces unavailable that should be present only
           // when active (  currently are all available ) also at inactive
           }
           return result;
           }
          
           template <class HardwareT>
           bool set_component_state(  HardwareT & hardware,   const rclcpp_lifecycle::State & target_state )
           {
           using lifecycle_msgs::msg::State;
          
           bool result = false;
          
           switch (  target_state.id(   ) )
           {
           case State::PRIMARY_STATE_UNCONFIGURED:
           switch (  hardware.get_state(   ).id(   ) )
           {
           case State::PRIMARY_STATE_UNCONFIGURED:
           result = true;
           break;
           case State::PRIMARY_STATE_INACTIVE:
           result = cleanup_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_ACTIVE:
           result = deactivate_hardware(  hardware );
           if (  result )
           {
           result = cleanup_hardware(  hardware );
           }
           break;
           case State::PRIMARY_STATE_FINALIZED:
           result = false;
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",   "hardware '%s' is in finalized state and can be only destroyed.",  
           hardware.get_name(   ).c_str(   ) );
           break;
           }
           break;
           case State::PRIMARY_STATE_INACTIVE:
           switch (  hardware.get_state(   ).id(   ) )
           {
           case State::PRIMARY_STATE_UNCONFIGURED:
           result = configure_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_INACTIVE:
           result = true;
           break;
           case State::PRIMARY_STATE_ACTIVE:
           result = deactivate_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_FINALIZED:
           result = false;
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",   "hardware '%s' is in finalized state and can be only destroyed.",  
           hardware.get_name(   ).c_str(   ) );
           break;
           }
           break;
           case State::PRIMARY_STATE_ACTIVE:
           switch (  hardware.get_state(   ).id(   ) )
           {
           case State::PRIMARY_STATE_UNCONFIGURED:
           result = configure_hardware(  hardware );
           if (  result )
           {
           result = activate_hardware(  hardware );
           }
           break;
           case State::PRIMARY_STATE_INACTIVE:
           result = activate_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_ACTIVE:
           result = true;
           break;
           case State::PRIMARY_STATE_FINALIZED:
           result = false;
           RCUTILS_LOG_WARN_NAMED(  
           "resource_manager",   "hardware '%s' is in finalized state and can be only destroyed.",  
           hardware.get_name(   ).c_str(   ) );
           break;
           }
           break;
           case State::PRIMARY_STATE_FINALIZED:
           switch (  hardware.get_state(   ).id(   ) )
           {
           case State::PRIMARY_STATE_UNCONFIGURED:
           result = shutdown_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_INACTIVE:
           result = shutdown_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_ACTIVE:
           result = shutdown_hardware(  hardware );
           break;
           case State::PRIMARY_STATE_FINALIZED:
           result = true;
           break;
           }
           break;
           }
          
           return result;
           }
          
           template <class HardwareT>
           void import_state_interfaces(  HardwareT & hardware )
           {
           auto interfaces = hardware.export_state_interfaces(   );
           std::vector<std::string> interface_names;
           interface_names.reserve(  interfaces.size(   ) );
           for (  auto & interface : interfaces )
           {
           auto key = interface.get_name(   );
           state_interface_map_.emplace(  std::make_pair(  key,   std::move(  interface ) ) );
           interface_names.push_back(  key );
           }
           hardware_info_map_[hardware.get_name(   )].state_interfaces = interface_names;
           available_state_interfaces_.reserve(  
           available_state_interfaces_.capacity(   ) + interface_names.size(   ) );
           }
          
           template <class HardwareT>
           void import_command_interfaces(  HardwareT & hardware )
           {
           auto interfaces = hardware.export_command_interfaces(   );
           hardware_info_map_[hardware.get_name(   )].command_interfaces = add_command_interfaces(  interfaces );
           }
          
           /// Adds exported command interfaces into internal storage.
           /**
           * Add command interfaces to the internal storage. Command interfaces exported from hardware or
           * chainable controllers are moved to the map with name-interface pairs,   the interface names are
           * added to the claimed map and available list's size is increased to reserve storage when
           * interface change theirs status in real-time control loop.
           *
           * \param[interfaces] list of command interface to add into storage.
           * \returns list of interface names that are added into internal storage. The output is used to
           * avoid additional iterations to cache interface names,   e.g.,   for initializing info structures.
           */
           std::vector<std::string> add_command_interfaces(  std::vector<CommandInterface> & interfaces )
           {
           std::vector<std::string> interface_names;
           interface_names.reserve(  interfaces.size(   ) );
           for (  auto & interface : interfaces )
           {
           auto key = interface.get_name(   );
           command_interface_map_.emplace(  std::make_pair(  key,   std::move(  interface ) ) );
           claimed_command_interface_map_.emplace(  std::make_pair(  key,   false ) );
           interface_names.push_back(  key );
           }
           available_command_interfaces_.reserve(  
           available_command_interfaces_.capacity(   ) + interface_names.size(   ) );
          
           return interface_names;
           }
          
           /// Removes command interfaces from internal storage.
           /**
           * Command interface are removed from the maps with theirs storage and their claimed status.
           *
           * \param[interface_names] list of command interface names to remove from storage.
           */
           void remove_command_interfaces(  const std::vector<std::string> & interface_names )
           {
           for (  const auto & interface : interface_names )
           {
           command_interface_map_.erase(  interface );
           claimed_command_interface_map_.erase(  interface );
           }
           }
          
           void check_for_duplicates(  const HardwareInfo & hardware_info )
           {
           // Check for identical names
           if (  hardware_info_map_.find(  hardware_info.name ) != hardware_info_map_.end(   ) )
           {
           throw std::runtime_error(  
           "Hardware name " + hardware_info.name +
           " is duplicated. Please provide a unique 'name' in the URDF." );
           }
           }
          
           // TODO(  destogl ): Propagate "false" up,   if happens in initialize_hardware
           void load_and_initialize_actuator(  const HardwareInfo & hardware_info )
           {
           check_for_duplicates(  hardware_info );
           load_hardware<Actuator,   ActuatorInterface>(  hardware_info,   actuator_loader_,   actuators_ );
           initialize_hardware(  hardware_info,   actuators_.back(   ) );
           import_state_interfaces(  actuators_.back(   ) );
           import_command_interfaces(  actuators_.back(   ) );
           }
          
           void load_and_initialize_sensor(  const HardwareInfo & hardware_info )
           {
           check_for_duplicates(  hardware_info );
           load_hardware<Sensor,   SensorInterface>(  hardware_info,   sensor_loader_,   sensors_ );
           initialize_hardware(  hardware_info,   sensors_.back(   ) );
           import_state_interfaces(  sensors_.back(   ) );
           }
          
           void load_and_initialize_system(  const HardwareInfo & hardware_info )
           {
           check_for_duplicates(  hardware_info );
           load_hardware<System,   SystemInterface>(  hardware_info,   system_loader_,   systems_ );
           initialize_hardware(  hardware_info,   systems_.back(   ) );
           import_state_interfaces(  systems_.back(   ) );
           import_command_interfaces(  systems_.back(   ) );
           }
          
           void initialize_actuator(  
           std::unique_ptr<ActuatorInterface> actuator,   const HardwareInfo & hardware_info )
           {
           this->actuators_.emplace_back(  Actuator(  std::move(  actuator ) ) );
           initialize_hardware(  hardware_info,   actuators_.back(   ) );
           import_state_interfaces(  actuators_.back(   ) );
           import_command_interfaces(  actuators_.back(   ) );
           }
          
           void initialize_sensor(  
           std::unique_ptr<SensorInterface> sensor,   const HardwareInfo & hardware_info )
           {
           this->sensors_.emplace_back(  Sensor(  std::move(  sensor ) ) );
           initialize_hardware(  hardware_info,   sensors_.back(   ) );
           import_state_interfaces(  sensors_.back(   ) );
           }
          
           void initialize_system(  
           std::unique_ptr<SystemInterface> system,   const HardwareInfo & hardware_info )
           {
           this->systems_.emplace_back(  System(  std::move(  system ) ) );
           initialize_hardware(  hardware_info,   systems_.back(   ) );
           import_state_interfaces(  systems_.back(   ) );
           import_command_interfaces(  systems_.back(   ) );
           }
          
           // hardware plugins
           pluginlib::ClassLoader<ActuatorInterface> actuator_loader_;
           pluginlib::ClassLoader<SensorInterface> sensor_loader_;
           pluginlib::ClassLoader<SystemInterface> system_loader_;
          
           std::vector<Actuator> actuators_;
           std::vector<Sensor> sensors_;
           std::vector<System> systems_;
          
           std::unordered_map<std::string,   HardwareComponentInfo> hardware_info_map_;
          
           std::unordered_map<std::string,   std::vector<std::string>> controllers_reference_interfaces_map_;
          
           /// Storage of all available state interfaces
           std::map<std::string,   StateInterface> state_interface_map_;
           /// Storage of all available command interfaces
           std::map<std::string,   CommandInterface> command_interface_map_;
          
           /// Vectors with interfaces available to controllers (  depending on hardware component state )
           std::vector<std::string> available_state_interfaces_;
           std::vector<std::string> available_command_interfaces_;
          
           /// List of all claimed command interfaces
           std::unordered_map<std::string,   bool> claimed_command_interface_map_;
          };
          
          ResourceManager::ResourceManager(   ) : resource_storage_(  std::make_unique<ResourceStorage>(   ) ) {}
          
          ResourceManager::~ResourceManager(   ) = default;
          
          ResourceManager::ResourceManager(  
           const std::string & urdf,   bool validate_interfaces,   bool activate_all )
          : resource_storage_(  std::make_unique<ResourceStorage>(   ) )
          {
           load_urdf(  urdf,   validate_interfaces );
          
           if (  activate_all )
           {
           for (  auto const & hw_info : resource_storage_->hardware_info_map_ )
           {
           using lifecycle_msgs::msg::State;
           rclcpp_lifecycle::State state(  State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE );
           set_component_state(  hw_info.first,   state );
           }
           }
          }
          
          void ResourceManager::load_urdf(  const std::string & urdf,   bool validate_interfaces )
          {
           const std::string system_type = "system";
           const std::string sensor_type = "sensor";
           const std::string actuator_type = "actuator";
          
           const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(  urdf );
           for (  const auto & individual_hardware_info : hardware_info )
           {
           if (  individual_hardware_info.type == actuator_type )
           {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           resource_storage_->load_and_initialize_actuator(  individual_hardware_info );
           }
           if (  individual_hardware_info.type == sensor_type )
           {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           resource_storage_->load_and_initialize_sensor(  individual_hardware_info );
           }
           if (  individual_hardware_info.type == system_type )
           {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           resource_storage_->load_and_initialize_system(  individual_hardware_info );
           }
           }
          
           // throw on missing state and command interfaces,   not specified keys are being ignored
           if (  validate_interfaces )
           {
           validate_storage(  hardware_info );
           }
          }
          
          LoanedStateInterface ResourceManager::claim_state_interface(  const std::string & key )
          {
           if (  !state_interface_is_available(  key ) )
           {
           throw std::runtime_error(  std::string(  "State interface with key '" ) + key + "' does not exist" );
           }
          
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return LoanedStateInterface(  resource_storage_->state_interface_map_.at(  key ) );
          }
          
          std::vector<std::string> ResourceManager::state_interface_keys(   ) const
          {
           std::vector<std::string> keys;
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           for (  const auto & item : resource_storage_->state_interface_map_ )
           {
           keys.push_back(  std::get<0>(  item ) );
           }
           return keys;
          }
          
          std::vector<std::string> ResourceManager::available_state_interfaces(   ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return resource_storage_->available_state_interfaces_;
          }
          
          bool ResourceManager::state_interface_exists(  const std::string & key ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return resource_storage_->state_interface_map_.find(  key ) !=
           resource_storage_->state_interface_map_.end(   );
          }
          
          bool ResourceManager::state_interface_is_available(  const std::string & name ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return std::find(  
           resource_storage_->available_state_interfaces_.begin(   ),  
           resource_storage_->available_state_interfaces_.end(   ),  
           name ) != resource_storage_->available_state_interfaces_.end(   );
          }
          
          void ResourceManager::import_controller_reference_interfaces(  
           const std::string & controller_name,   std::vector<CommandInterface> & interfaces )
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           auto interface_names = resource_storage_->add_command_interfaces(  interfaces );
           resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names;
          }
          
          std::vector<std::string> ResourceManager::get_controller_reference_interface_names(  
           const std::string & controller_name )
          {
           return resource_storage_->controllers_reference_interfaces_map_.at(  controller_name );
          }
          
          void ResourceManager::make_controller_reference_interfaces_available(  
           const std::string & controller_name )
          {
           auto interface_names =
           resource_storage_->controllers_reference_interfaces_map_.at(  controller_name );
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           resource_storage_->available_command_interfaces_.insert(  
           resource_storage_->available_command_interfaces_.end(   ),   interface_names.begin(   ),  
           interface_names.end(   ) );
          }
          
          void ResourceManager::make_controller_reference_interfaces_unavailable(  
           const std::string & controller_name )
          {
           auto interface_names =
           resource_storage_->controllers_reference_interfaces_map_.at(  controller_name );
          
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           for (  const auto & interface : interface_names )
           {
           auto found_it = std::find(  
           resource_storage_->available_command_interfaces_.begin(   ),  
           resource_storage_->available_command_interfaces_.end(   ),   interface );
           if (  found_it != resource_storage_->available_command_interfaces_.end(   ) )
           {
           resource_storage_->available_command_interfaces_.erase(  found_it );
           RCUTILS_LOG_DEBUG_NAMED(  
           "resource_manager",   "'%s' command interface removed from available list",  
           interface.c_str(   ) );
           }
           }
          }
          
          void ResourceManager::remove_controller_reference_interfaces(  const std::string & controller_name )
          {
           auto interface_names =
           resource_storage_->controllers_reference_interfaces_map_.at(  controller_name );
           resource_storage_->controllers_reference_interfaces_map_.erase(  controller_name );
          
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           resource_storage_->remove_command_interfaces(  interface_names );
          }
          
          // CM API: Called in "update"-thread
          bool ResourceManager::command_interface_is_claimed(  const std::string & key ) const
          {
           if (  !command_interface_is_available(  key ) )
           {
           return false;
           }
          
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           return resource_storage_->claimed_command_interface_map_.at(  key );
          }
          
          // CM API: Called in "update"-thread
          LoanedCommandInterface ResourceManager::claim_command_interface(  const std::string & key )
          {
           if (  !command_interface_is_available(  key ) )
           {
           throw std::runtime_error(  std::string(  "Command interface with '" ) + key + "' does not exist" );
           }
          
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           if (  command_interface_is_claimed(  key ) )
           {
           throw std::runtime_error(  
           std::string(  "Command interface with '" ) + key + "' is already claimed" );
           }
          
           resource_storage_->claimed_command_interface_map_[key] = true;
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return LoanedCommandInterface(  
           resource_storage_->command_interface_map_.at(  key ),  
           std::bind(  &ResourceManager::release_command_interface,   this,   key ) );
          }
          
          // CM API: Called in "update"-thread
          void ResourceManager::release_command_interface(  const std::string & key )
          {
           std::lock_guard<std::recursive_mutex> guard_claimed(  claimed_command_interfaces_lock_ );
           resource_storage_->claimed_command_interface_map_[key] = false;
          }
          
          std::vector<std::string> ResourceManager::command_interface_keys(   ) const
          {
           std::vector<std::string> keys;
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           for (  const auto & item : resource_storage_->command_interface_map_ )
           {
           keys.push_back(  std::get<0>(  item ) );
           }
           return keys;
          }
          
          std::vector<std::string> ResourceManager::available_command_interfaces(   ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return resource_storage_->available_command_interfaces_;
          }
          
          bool ResourceManager::command_interface_exists(  const std::string & key ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return resource_storage_->command_interface_map_.find(  key ) !=
           resource_storage_->command_interface_map_.end(   );
          }
          
          // CM API
          bool ResourceManager::command_interface_is_available(  const std::string & name ) const
          {
           std::lock_guard<std::recursive_mutex> guard(  resource_interfaces_lock_ );
           return std::find(  
           resource_storage_->available_command_interfaces_.begin(   ),  
           resource_storage_->available_command_interfaces_.end(   ),  
           name ) != resource_storage_->available_command_interfaces_.end(   );
          }
          
          size_t ResourceManager::actuator_components_size(   ) const
          {
           return resource_storage_->actuators_.size(   );
          }
          
          size_t ResourceManager::sensor_components_size(   ) const
          {
           return resource_storage_->sensors_.size(   );
          }
          
          void ResourceManager::import_component(  
           std::unique_ptr<ActuatorInterface> actuator,   const HardwareInfo & hardware_info )
          {
           resource_storage_->initialize_actuator(  std::move(  actuator ),   hardware_info );
          }
          
          void ResourceManager::import_component(  
           std::unique_ptr<SensorInterface> sensor,   const HardwareInfo & hardware_info )
          {
           resource_storage_->initialize_sensor(  std::move(  sensor ),   hardware_info );
          }
          
          void ResourceManager::import_component(  
           std::unique_ptr<SystemInterface> system,   const HardwareInfo & hardware_info )
          {
           resource_storage_->initialize_system(  std::move(  system ),   hardware_info );
          }
          
          size_t ResourceManager::system_components_size(   ) const
          {
           return resource_storage_->systems_.size(   );
          }
          // End of "used only in tests"
          
          std::unordered_map<std::string,   HardwareComponentInfo> ResourceManager::get_components_status(   )
          {
           for (  auto & component : resource_storage_->actuators_ )
           {
           resource_storage_->hardware_info_map_[component.get_name(   )].state = component.get_state(   );
           }
           for (  auto & component : resource_storage_->sensors_ )
           {
           resource_storage_->hardware_info_map_[component.get_name(   )].state = component.get_state(   );
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           resource_storage_->hardware_info_map_[component.get_name(   )].state = component.get_state(   );
           }
          
           return resource_storage_->hardware_info_map_;
          }
          
          bool ResourceManager::prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces )
          {
           auto interfaces_to_string = [&](   )
           {
           std::stringstream ss;
           ss << "Start interfaces: " << std::endl << "[" << std::endl;
           for (  const auto & start_if : start_interfaces )
           {
           ss << " " << start_if << std::endl;
           }
           ss << "]" << std::endl;
           ss << "Stop interfaces: " << std::endl << "[" << std::endl;
           for (  const auto & stop_if : stop_interfaces )
           {
           ss << " " << stop_if << std::endl;
           }
           ss << "]" << std::endl;
           return ss.str(   );
           };
          
           for (  auto & component : resource_storage_->actuators_ )
           {
           if (  return_type::OK != component.prepare_command_mode_switch(  start_interfaces,   stop_interfaces ) )
           {
           RCUTILS_LOG_ERROR_NAMED(  
           "resource_manager",   "Component '%s' did not accept new command resource combination: \n %s",  
           component.get_name(   ).c_str(   ),   interfaces_to_string(   ).c_str(   ) );
           return false;
           }
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           if (  return_type::OK != component.prepare_command_mode_switch(  start_interfaces,   stop_interfaces ) )
           {
           RCUTILS_LOG_ERROR_NAMED(  
           "resource_manager",   "Component '%s' did not accept new command resource combination: \n %s",  
           component.get_name(   ).c_str(   ),   interfaces_to_string(   ).c_str(   ) );
           return false;
           }
           }
           return true;
          }
          
          bool ResourceManager::perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces )
          {
           for (  auto & component : resource_storage_->actuators_ )
           {
           if (  return_type::OK != component.perform_command_mode_switch(  start_interfaces,   stop_interfaces ) )
           {
           RCUTILS_LOG_ERROR_NAMED(  
           "resource_manager",   "Component '%s' could not perform switch",  
           component.get_name(   ).c_str(   ) );
           return false;
           }
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           if (  return_type::OK != component.perform_command_mode_switch(  start_interfaces,   stop_interfaces ) )
           {
           RCUTILS_LOG_ERROR_NAMED(  
           "resource_manager",   "Component '%s' could not perform switch",  
           component.get_name(   ).c_str(   ) );
           return false;
           }
           }
           return true;
          }
          
          return_type ResourceManager::set_component_state(  
           const std::string & component_name,   rclcpp_lifecycle::State & target_state )
          {
           using lifecycle_msgs::msg::State;
           using std::placeholders::_1;
           using std::placeholders::_2;
          
           auto found_it = resource_storage_->hardware_info_map_.find(  component_name );
          
           if (  found_it == resource_storage_->hardware_info_map_.end(   ) )
           {
           RCUTILS_LOG_INFO_NAMED(  
           "resource_manager",   "Hardware Component with name '%s' does not exists",  
           component_name.c_str(   ) );
           return return_type::ERROR;
           }
          
           return_type result = return_type::OK;
          
           if (  target_state.id(   ) == 0 )
           {
           if (  target_state.label(   ) == lifecycle_state_names::UNCONFIGURED )
           {
           target_state = rclcpp_lifecycle::State(  
           State::PRIMARY_STATE_UNCONFIGURED,   lifecycle_state_names::UNCONFIGURED );
           }
           if (  target_state.label(   ) == lifecycle_state_names::INACTIVE )
           {
           target_state =
           rclcpp_lifecycle::State(  State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE );
           }
           if (  target_state.label(   ) == lifecycle_state_names::ACTIVE )
           {
           target_state =
           rclcpp_lifecycle::State(  State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE );
           }
           if (  target_state.label(   ) == lifecycle_state_names::FINALIZED )
           {
           target_state =
           rclcpp_lifecycle::State(  State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED );
           }
           }
          
           auto find_set_component_state = [&](  auto action,   auto & components )
           {
           auto found_component_it = std::find_if(  
           components.begin(   ),   components.end(   ),  
           [&](  const auto & component ) { return component.get_name(   ) == component_name; } );
          
           if (  found_component_it != components.end(   ) )
           {
           if (  action(  *found_component_it,   target_state ) )
           {
           result = return_type::OK;
           }
           else
           {
           result = return_type::ERROR;
           }
           return true;
           }
           return false;
           };
          
           bool found = find_set_component_state(  
           std::bind(  &ResourceStorage::set_component_state<Actuator>,   resource_storage_.get(   ),   _1,   _2 ),  
           resource_storage_->actuators_ );
           if (  !found )
           {
           found = find_set_component_state(  
           std::bind(  &ResourceStorage::set_component_state<Sensor>,   resource_storage_.get(   ),   _1,   _2 ),  
           resource_storage_->sensors_ );
           }
           if (  !found )
           {
           found = find_set_component_state(  
           std::bind(  &ResourceStorage::set_component_state<System>,   resource_storage_.get(   ),   _1,   _2 ),  
           resource_storage_->systems_ );
           }
          
           return result;
          }
          
          void ResourceManager::read(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           for (  auto & component : resource_storage_->actuators_ )
           {
           component.read(  time,   period );
           }
           for (  auto & component : resource_storage_->sensors_ )
           {
           component.read(  time,   period );
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           component.read(  time,   period );
           }
          }
          
          void ResourceManager::write(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           for (  auto & component : resource_storage_->actuators_ )
           {
           component.write(  time,   period );
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           component.write(  time,   period );
           }
          }
          
          void ResourceManager::validate_storage(  
           const std::vector<hardware_interface::HardwareInfo> & hardware_info ) const
          {
           std::vector<std::string> missing_state_keys = {};
           std::vector<std::string> missing_command_keys = {};
          
           for (  const auto & hardware : hardware_info )
           {
           for (  const auto & joint : hardware.joints )
           {
           for (  const auto & state_interface : joint.state_interfaces )
           {
           if (  !state_interface_exists(  joint.name + "/" + state_interface.name ) )
           {
           missing_state_keys.emplace_back(  joint.name + "/" + state_interface.name );
           }
           }
           for (  const auto & command_interface : joint.command_interfaces )
           {
           if (  !command_interface_exists(  joint.name + "/" + command_interface.name ) )
           {
           missing_command_keys.emplace_back(  joint.name + "/" + command_interface.name );
           }
           }
           }
           for (  const auto & sensor : hardware.sensors )
           {
           for (  const auto & state_interface : sensor.state_interfaces )
           {
           if (  !state_interface_exists(  sensor.name + "/" + state_interface.name ) )
           {
           missing_state_keys.emplace_back(  sensor.name + "/" + state_interface.name );
           }
           }
           }
           }
          
           if (  !missing_state_keys.empty(   ) || !missing_command_keys.empty(   ) )
           {
           std::string err_msg = "Wrong state or command interface configuration.\n";
           err_msg += "missing state interfaces:\n";
           for (  const auto & missing_key : missing_state_keys )
           {
           err_msg += "' " + missing_key + " '" + "\t";
           }
           err_msg += "\nmissing command interfaces:\n";
           for (  const auto & missing_key : missing_command_keys )
           {
           err_msg += "' " + missing_key + " '" + "\t";
           }
          
           throw std::runtime_error(  err_msg );
           }
          }
          
          // Temporary method to keep old interface and reduce framework changes in PRs
          void ResourceManager::activate_all_components(   )
          {
           using lifecycle_msgs::msg::State;
           rclcpp_lifecycle::State active_state(  
           State::PRIMARY_STATE_ACTIVE,   hardware_interface::lifecycle_state_names::ACTIVE );
          
           for (  auto & component : resource_storage_->actuators_ )
           {
           set_component_state(  component.get_name(   ),   active_state );
           }
           for (  auto & component : resource_storage_->sensors_ )
           {
           set_component_state(  component.get_name(   ),   active_state );
           }
           for (  auto & component : resource_storage_->systems_ )
           {
           set_component_state(  component.get_name(   ),   active_state );
           }
          }
          
          } // namespace hardware_interface

ros2_control/hardware_interface/src/sensor.cpp

       1  // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "hardware_interface/sensor.hpp"
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/sensor_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      35  Sensor::Sensor(  std::unique_ptr<SensorInterface> impl ) : impl_(  std::move(  impl ) ) {}
          
      37  const rclcpp_lifecycle::State & Sensor::initialize(  const HardwareInfo & sensor_info )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_init(  sensor_info ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      58  const rclcpp_lifecycle::State & Sensor::configure(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
           {
           switch (  impl_->on_configure(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      81  const rclcpp_lifecycle::State & Sensor::cleanup(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_cleanup(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     101  const rclcpp_lifecycle::State & Sensor::shutdown(   )
          {
           if (  
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
           {
           switch (  impl_->on_shutdown(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     122  const rclcpp_lifecycle::State & Sensor::activate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_activate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     144  const rclcpp_lifecycle::State & Sensor::deactivate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           switch (  impl_->on_deactivate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     166  const rclcpp_lifecycle::State & Sensor::error(   )
          {
           if (  impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_error(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     187  std::vector<StateInterface> Sensor::export_state_interfaces(   )
          {
           return impl_->export_state_interfaces(   );
          }
          
     192  std::string Sensor::get_name(   ) const { return impl_->get_name(   ); }
          
     194  const rclcpp_lifecycle::State & Sensor::get_state(   ) const { return impl_->get_state(   ); }
          
     196  return_type Sensor::read(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type result = return_type::ERROR;
           if (  
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = impl_->read(  time,   period );
           if (  result == return_type::ERROR )
           {
           error(   );
           }
           }
           return result;
          }
          
          } // namespace hardware_interface

ros2_control/hardware_interface/src/system.cpp

       1  // Copyright 2020 - 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "hardware_interface/system.hpp"
          
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          
          namespace hardware_interface
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
      35  System::System(  std::unique_ptr<SystemInterface> impl ) : impl_(  std::move(  impl ) ) {}
          
      37  const rclcpp_lifecycle::State & System::initialize(  const HardwareInfo & system_info )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_init(  system_info ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      58  const rclcpp_lifecycle::State & System::configure(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
           {
           switch (  impl_->on_configure(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
      81  const rclcpp_lifecycle::State & System::cleanup(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_cleanup(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     101  const rclcpp_lifecycle::State & System::shutdown(   )
          {
           if (  
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
           impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
           {
           switch (  impl_->on_shutdown(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     122  const rclcpp_lifecycle::State & System::activate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           switch (  impl_->on_activate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     144  const rclcpp_lifecycle::State & System::deactivate(   )
          {
           if (  impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           switch (  impl_->on_deactivate(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   lifecycle_state_names::INACTIVE ) );
           break;
           case CallbackReturn::FAILURE:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   lifecycle_state_names::ACTIVE ) );
           break;
           case CallbackReturn::ERROR:
           impl_->set_state(  error(   ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     166  const rclcpp_lifecycle::State & System::error(   )
          {
           if (  impl_->get_state(   ).id(   ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
           {
           switch (  impl_->on_error(  impl_->get_state(   ) ) )
           {
           case CallbackReturn::SUCCESS:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           lifecycle_state_names::UNCONFIGURED ) );
           break;
           case CallbackReturn::FAILURE:
           case CallbackReturn::ERROR:
           impl_->set_state(  rclcpp_lifecycle::State(  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   lifecycle_state_names::FINALIZED ) );
           break;
           }
           }
           return impl_->get_state(   );
          }
          
     187  std::vector<StateInterface> System::export_state_interfaces(   )
          {
           return impl_->export_state_interfaces(   );
          }
          
     192  std::vector<CommandInterface> System::export_command_interfaces(   )
          {
           return impl_->export_command_interfaces(   );
          }
          
     197  return_type System::prepare_command_mode_switch(  
     198   const std::vector<std::string> & start_interfaces,  
     199   const std::vector<std::string> & stop_interfaces )
          {
           return impl_->prepare_command_mode_switch(  start_interfaces,   stop_interfaces );
          }
          
     204  return_type System::perform_command_mode_switch(  
     205   const std::vector<std::string> & start_interfaces,  
     206   const std::vector<std::string> & stop_interfaces )
          {
           return impl_->perform_command_mode_switch(  start_interfaces,   stop_interfaces );
          }
          
     211  std::string System::get_name(   ) const { return impl_->get_name(   ); }
          
     213  const rclcpp_lifecycle::State & System::get_state(   ) const { return impl_->get_state(   ); }
          
     215  return_type System::read(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type result = return_type::ERROR;
           if (  
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = impl_->read(  time,   period );
           if (  result == return_type::ERROR )
           {
           error(   );
           }
           }
           return result;
          }
          
     231  return_type System::write(  const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           return_type result = return_type::ERROR;
           if (  
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
           impl_->get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
           {
           result = impl_->write(  time,   period );
           if (  result == return_type::ERROR )
           {
           error(   );
           }
           }
           return result;
          }
          
          } // namespace hardware_interface

ros2_control/hardware_interface/test/mock_components/test_generic_system.cpp

          // Copyright (  c ) 2021 PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          // Author: Denis Stogl
          
          #include <gmock/gmock.h>
          
          #include <cmath>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "ros2_control_test_assets/components_urdfs.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
          namespace
          {
      35  const auto TIME = rclcpp::Time(  0 );
          const auto PERIOD = rclcpp::Duration::from_seconds(  0.01 );
          } // namespace
          
          class TestGenericSystem : public ::testing::Test
          {
          protected:
           void SetUp(   ) override
           {
           // REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED
           hardware_fake_system_2dof_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>fake_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <param name="initial_position">1.57</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <param name="initial_position">0.7854</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <param name="initial_position">1.57</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <param name="initial_position">0.7854</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_asymetric_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">1.57</param>
           </joint>
           <joint name="joint2">
           <command_interface name="acceleration"/>
           <state_interface name="position"/>
           <param name="initial_position">0.7854</param>
           <param name="initial_acceleration">0.8554</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_standard_interfaces_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">3.45</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">2.78</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_with_other_interface_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">1.55</param>
           <param name="initial_velocity">0.1</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">0.65</param>
           <param name="initial_velocity">0.2</param>
           </joint>
           <joint name="voltage_output">
           <command_interface name="voltage"/>
           <state_interface name="voltage"/>
           <param name="initial_voltage">0.5</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_with_sensor_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <sensor name="tcp_force_sensor">
           <state_interface name="fx"/>
           <state_interface name="fy"/>
           <state_interface name="tx"/>
           <state_interface name="ty"/>
           <param name="frame_id">kuka_tcp</param>
           </sensor>
           </ros2_control>
           )";
          
           hardware_system_2dof_with_sensor_fake_command_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="fake_sensor_commands">true</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <sensor name="tcp_force_sensor">
           <state_interface name="fx"/>
           <state_interface name="fy"/>
           <state_interface name="tx"/>
           <state_interface name="ty"/>
           <param name="frame_id">kuka_tcp</param>
           </sensor>
           </ros2_control>
           )";
          
           hardware_system_2dof_with_sensor_fake_command_True_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="fake_sensor_commands">True</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <sensor name="tcp_force_sensor">
           <state_interface name="fx"/>
           <state_interface name="fy"/>
           <state_interface name="tx"/>
           <state_interface name="ty"/>
           <param name="frame_id">kuka_tcp</param>
           </sensor>
           </ros2_control>
           )";
          
           hardware_system_2dof_with_mimic_joint_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">1.57</param>
           </joint>
           <joint name="joint2">
           <param name="mimic">joint1</param>
           <param name="multiplier">-2</param>
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_standard_interfaces_with_offset_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="position_state_following_offset">-3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position">
           <param name="initial_value">3.45</param>
           </state_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position">
           <param name="initial_value">2.78</param>
           </state_interface>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="position_state_following_offset">-3</param>
           <param name="custom_interface_with_following_offset">actual_position</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">3.45</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">2.78</param>
           </joint>
           </ros2_control>
           )";
          
           hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="position_state_following_offset">-3</param>
           <param name="custom_interface_with_following_offset">actual_position</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="actual_position"/>
           <param name="initial_position">3.45</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="actual_position"/>
           <param name="initial_position">2.78</param>
           </joint>
           </ros2_control>
           )";
          
           valid_urdf_ros2_control_system_robot_with_gpio_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position">
           <param name="initial_value">3.45</param>
           </state_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">2.78</param>
           </joint>
           <gpio name="flange_analog_IOs">
           <command_interface name="analog_output1" data_type="double"/>
           <state_interface name="analog_output1"/>
           <state_interface name="analog_input1"/>
           <state_interface name="analog_input2"/>
           </gpio>
           <gpio name="flange_vacuum">
           <command_interface name="vacuum"/>
           <state_interface name="vacuum" data_type="double"/>
           </gpio>
           </ros2_control>
           )";
          
           valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ =
           R"(  
           <ros2_control name="GenericSystem2dof" type="system">
           <hardware>
           <plugin>mock_components/GenericSystem</plugin>
           <param name="fake_gpio_commands">True</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">3.45</param>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <param name="initial_position">2.78</param>
           </joint>
           <gpio name="flange_analog_IOs">
           <command_interface name="analog_output1" data_type="double"/>
           <state_interface name="analog_output1"/>
           <state_interface name="analog_input1"/>
           <state_interface name="analog_input2"/>
           </gpio>
           <gpio name="flange_vacuum">
           <command_interface name="vacuum"/>
           <state_interface name="vacuum" data_type="double"/>
           </gpio>
           </ros2_control>
           )";
           }
          
           std::string hardware_robot_2dof_;
           std::string hardware_system_2dof_;
           std::string hardware_fake_system_2dof_;
           std::string hardware_system_2dof_asymetric_;
           std::string hardware_system_2dof_standard_interfaces_;
           std::string hardware_system_2dof_with_other_interface_;
           std::string hardware_system_2dof_with_sensor_;
           std::string hardware_system_2dof_with_sensor_fake_command_;
           std::string hardware_system_2dof_with_sensor_fake_command_True_;
           std::string hardware_system_2dof_with_mimic_joint_;
           std::string hardware_system_2dof_standard_interfaces_with_offset_;
           std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_;
           std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_;
           std::string valid_urdf_ros2_control_system_robot_with_gpio_;
           std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_;
          };
          
          void set_components_state(  
           hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components,  
           const uint8_t state_id,   const std::string & state_name )
          {
           for (  const auto & component : components )
           {
           rclcpp_lifecycle::State state(  state_id,   state_name );
           rm.set_component_state(  component,   state );
           }
          }
          
          auto configure_components = [](  
           hardware_interface::ResourceManager & rm,  
           const std::vector<std::string> & components = {"GenericSystem2dof"} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           hardware_interface::lifecycle_state_names::INACTIVE );
          };
          
          auto activate_components = [](  
           hardware_interface::ResourceManager & rm,  
           const std::vector<std::string> & components = {"GenericSystem2dof"} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           hardware_interface::lifecycle_state_names::ACTIVE );
          };
          
          auto deactivate_components = [](  
           hardware_interface::ResourceManager & rm,  
           const std::vector<std::string> & components = {"GenericSystem2dof"} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           hardware_interface::lifecycle_state_names::INACTIVE );
          };
          
          TEST_F(  TestGenericSystem,   load_generic_system_2dof )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
           ros2_control_test_assets::urdf_tail;
           ASSERT_NO_THROW(  hardware_interface::ResourceManager rm(  urdf ) );
          }
          
          // REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED
          TEST_F(  TestGenericSystem,   generic_fake_system_2dof_symetric_interfaces )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  2u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
          
           ASSERT_EQ(  2u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
          
           ASSERT_EQ(  1.57,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.7854,   j2p_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
          }
          
          // Test inspired by hardware_interface/test_resource_manager.cpp
          TEST_F(  TestGenericSystem,   generic_system_2dof_symetric_interfaces )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  2u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
          
           ASSERT_EQ(  2u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
          
           ASSERT_EQ(  1.57,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.7854,   j2p_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
          }
          
          // Test inspired by hardware_interface/test_resource_manager.cpp
          TEST_F(  TestGenericSystem,   generic_system_2dof_asymetric_interfaces )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  2u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_FALSE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_FALSE(  rm.state_interface_exists(  "joint1/acceleration" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_FALSE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_FALSE(  rm.state_interface_exists(  "joint2/acceleration" ) );
          
           ASSERT_EQ(  2u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "joint1/acceleration" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/acceleration" ) );
          
           // Check initial values
           ASSERT_ANY_THROW(  rm.claim_state_interface(  "joint1/position" ) );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           ASSERT_ANY_THROW(  rm.claim_state_interface(  "joint1/acceleration" ) );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           ASSERT_ANY_THROW(  rm.claim_state_interface(  "joint2/velocity" ) );
           ASSERT_ANY_THROW(  rm.claim_state_interface(  "joint2/acceleration" ) );
          
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           ASSERT_ANY_THROW(  rm.claim_command_interface(  "joint1/velocity" ) );
           ASSERT_ANY_THROW(  rm.claim_command_interface(  "joint1/acceleration" ) );
           ASSERT_ANY_THROW(  rm.claim_command_interface(  "joint2/position" ) );
           ASSERT_ANY_THROW(  rm.claim_command_interface(  "joint2/position" ) );
           hardware_interface::LoanedCommandInterface j2a_c =
           rm.claim_command_interface(  "joint2/acceleration" );
          
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.7854,   j2p_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2a_c.get_value(   ) ) );
          }
          
          void generic_system_functional_test(  const std::string & urdf,   const double offset = 0 )
          {
           hardware_interface::ResourceManager rm(  urdf );
           // check is hardware is configured
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           configure_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           activate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface(  "joint1/velocity" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface(  "joint2/velocity" );
          
           // State interfaces without initial value are set to 0
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j1v_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2v_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j1v_c.set_value(  0.22 );
           j2p_c.set_value(  0.33 );
           j2v_c.set_value(  0.44 );
          
           // State values should not be changed
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // read(   ) mirrors commands + offset to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11 + offset,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33 + offset,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.55 );
           j1v_c.set_value(  0.66 );
           j2p_c.set_value(  0.77 );
           j2v_c.set_value(  0.88 );
          
           // state values should not be changed
           ASSERT_EQ(  0.11 + offset,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33 + offset,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.55,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.66,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.77,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.88,   j2v_c.get_value(   ) );
          
           deactivate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_functionality )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ +
           ros2_control_test_assets::urdf_tail;
          
           generic_system_functional_test(  urdf );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_other_interfaces )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  5u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "voltage_output/voltage" ) );
          
           ASSERT_EQ(  5u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "voltage_output/voltage" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedStateInterface vo_s =
           rm.claim_state_interface(  "voltage_output/voltage" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface vo_c =
           rm.claim_command_interface(  "voltage_output/voltage" );
          
           ASSERT_EQ(  1.55,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.1,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.65,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.2,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.5,   vo_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  vo_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j2p_c.set_value(  0.33 );
           vo_c.set_value(  0.99 );
          
           // State values should not be changed
           ASSERT_EQ(  1.55,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.1,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.65,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.2,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.5,   vo_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.99,   vo_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  1.55,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.1,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.65,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.2,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.5,   vo_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.99,   vo_c.get_value(   ) );
          
           // read(   ) mirrors commands to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.1,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.99,   vo_s.get_value(   ) );
           ASSERT_EQ(  0.2,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.99,   vo_c.get_value(   ) );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_sensor )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  8u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/fx" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/fy" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/tx" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/ty" ) );
          
           ASSERT_EQ(  4u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "tcp_force_sensor/fx" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "tcp_force_sensor/fy" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "tcp_force_sensor/tx" ) );
           EXPECT_FALSE(  rm.command_interface_exists(  "tcp_force_sensor/ty" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedStateInterface sfx_s = rm.claim_state_interface(  "tcp_force_sensor/fx" );
           hardware_interface::LoanedStateInterface sfy_s = rm.claim_state_interface(  "tcp_force_sensor/fy" );
           hardware_interface::LoanedStateInterface stx_s = rm.claim_state_interface(  "tcp_force_sensor/tx" );
           hardware_interface::LoanedStateInterface sty_s = rm.claim_state_interface(  "tcp_force_sensor/ty" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
           EXPECT_ANY_THROW(  rm.claim_command_interface(  "tcp_force_sensor/fx" ) );
           EXPECT_ANY_THROW(  rm.claim_command_interface(  "tcp_force_sensor/fy" ) );
           EXPECT_ANY_THROW(  rm.claim_command_interface(  "tcp_force_sensor/tx" ) );
           EXPECT_ANY_THROW(  rm.claim_command_interface(  "tcp_force_sensor/ty" ) );
          
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j2p_c.set_value(  0.33 );
          
           // State values should not be changed
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
          
           // read(   ) mirrors commands to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
          }
          
          void test_generic_system_with_fake_sensor_commands(  std::string urdf )
          {
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  8u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/fx" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/fy" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/tx" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "tcp_force_sensor/ty" ) );
          
           ASSERT_EQ(  8u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "tcp_force_sensor/fx" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "tcp_force_sensor/fy" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "tcp_force_sensor/tx" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "tcp_force_sensor/ty" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedStateInterface sfx_s = rm.claim_state_interface(  "tcp_force_sensor/fx" );
           hardware_interface::LoanedStateInterface sfy_s = rm.claim_state_interface(  "tcp_force_sensor/fy" );
           hardware_interface::LoanedStateInterface stx_s = rm.claim_state_interface(  "tcp_force_sensor/tx" );
           hardware_interface::LoanedStateInterface sty_s = rm.claim_state_interface(  "tcp_force_sensor/ty" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface sfx_c =
           rm.claim_command_interface(  "tcp_force_sensor/fx" );
           hardware_interface::LoanedCommandInterface sfy_c =
           rm.claim_command_interface(  "tcp_force_sensor/fy" );
           hardware_interface::LoanedCommandInterface stx_c =
           rm.claim_command_interface(  "tcp_force_sensor/tx" );
           hardware_interface::LoanedCommandInterface sty_c =
           rm.claim_command_interface(  "tcp_force_sensor/ty" );
          
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfx_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j2p_c.set_value(  0.33 );
           sfx_c.set_value(  1.11 );
           sfy_c.set_value(  2.22 );
           stx_c.set_value(  3.33 );
           sty_c.set_value(  4.44 );
          
           // State values should not be changed
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  1.11,   sfx_c.get_value(   ) );
           ASSERT_EQ(  2.22,   sfy_c.get_value(   ) );
           ASSERT_EQ(  3.33,   stx_c.get_value(   ) );
           ASSERT_EQ(  4.44,   sty_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  0.0,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           EXPECT_TRUE(  std::isnan(  sfx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sfy_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  stx_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  sty_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  1.11,   sfx_c.get_value(   ) );
           ASSERT_EQ(  2.22,   sfy_c.get_value(   ) );
           ASSERT_EQ(  3.33,   stx_c.get_value(   ) );
           ASSERT_EQ(  4.44,   sty_c.get_value(   ) );
          
           // read(   ) mirrors commands to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  1.11,   sfx_s.get_value(   ) );
           ASSERT_EQ(  2.22,   sfy_s.get_value(   ) );
           ASSERT_EQ(  3.33,   stx_s.get_value(   ) );
           ASSERT_EQ(  4.44,   sty_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  1.11,   sfx_c.get_value(   ) );
           ASSERT_EQ(  2.22,   sfy_c.get_value(   ) );
           ASSERT_EQ(  3.33,   stx_c.get_value(   ) );
           ASSERT_EQ(  4.44,   sty_c.get_value(   ) );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_sensor_fake_command )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ +
           ros2_control_test_assets::urdf_tail;
          
           test_generic_system_with_fake_sensor_commands(  urdf );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_sensor_fake_command_True )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           hardware_system_2dof_with_sensor_fake_command_True_ +
           ros2_control_test_assets::urdf_tail;
          
           test_generic_system_with_fake_sensor_commands(  urdf );
          }
          
          void test_generic_system_with_mimic_joint(  std::string urdf )
          {
           hardware_interface::ResourceManager rm(  urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  4u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
          
           ASSERT_EQ(  4u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface(  "joint1/velocity" );
          
           ASSERT_EQ(  1.57,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j1v_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j1v_c.set_value(  0.05 );
          
           // State values should not be changed
           ASSERT_EQ(  1.57,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.05,   j1v_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  1.57,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.05,   j1v_c.get_value(   ) );
          
           // read(   ) mirrors commands to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.05,   j1v_s.get_value(   ) );
           ASSERT_EQ(  -0.22,   j2p_s.get_value(   ) );
           ASSERT_EQ(  -0.1,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.05,   j1v_c.get_value(   ) );
          }
          
          TEST_F(  TestGenericSystem,   hardware_system_2dof_with_mimic_joint )
          {
           auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ +
           ros2_control_test_assets::urdf_tail;
          
           test_generic_system_with_mimic_joint(  urdf );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_functionality_with_offset )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           hardware_system_2dof_standard_interfaces_with_offset_ +
           ros2_control_test_assets::urdf_tail;
          
           generic_system_functional_test(  urdf,   -3 );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_functionality_with_offset_custom_interface_missing )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ +
           ros2_control_test_assets::urdf_tail;
          
           // custom interface is missing so offset will not be applied
           generic_system_functional_test(  urdf,   0.0 );
          }
          
          TEST_F(  TestGenericSystem,   generic_system_2dof_functionality_with_offset_custom_interface )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ +
           ros2_control_test_assets::urdf_tail;
          
           const double offset = -3;
          
           hardware_interface::ResourceManager rm(  urdf );
          
           // check is hardware is configured
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
          
           configure_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           activate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
          
           // Check initial values
           hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface(  "joint1/position" );
           hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface(  "joint1/velocity" );
           hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface(  "joint2/position" );
           hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface(  "joint2/velocity" );
           hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface(  "joint1/position" );
           hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface(  "joint1/velocity" );
           hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface(  "joint2/position" );
           hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface(  "joint2/velocity" );
          
           // set default value of custom state interfaces to anything first
           hardware_interface::LoanedStateInterface c_j1p_s =
           rm.claim_state_interface(  "joint1/actual_position" );
           hardware_interface::LoanedStateInterface c_j2p_s =
           rm.claim_state_interface(  "joint2/actual_position" );
          
           // State interfaces without initial value are set to 0
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_TRUE(  std::isnan(  j1p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j1v_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2p_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  j2v_c.get_value(   ) ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.11 );
           j1v_c.set_value(  0.22 );
           j2p_c.set_value(  0.33 );
           j2v_c.set_value(  0.44 );
          
           // State values should not be changed
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_EQ(  3.45,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j1v_s.get_value(   ) );
           ASSERT_EQ(  2.78,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.0,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // read(   ) mirrors commands + offset to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.11 + offset,   c_j1p_s.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.33 + offset,   c_j2p_s.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.11,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_c.get_value(   ) );
          
           // set some new values in commands
           j1p_c.set_value(  0.55 );
           j1v_c.set_value(  0.66 );
           j2p_c.set_value(  0.77 );
           j2v_c.set_value(  0.88 );
          
           // state values should not be changed
           ASSERT_EQ(  0.11,   j1p_s.get_value(   ) );
           ASSERT_EQ(  0.11 + offset,   c_j1p_s.get_value(   ) );
           ASSERT_EQ(  0.22,   j1v_s.get_value(   ) );
           ASSERT_EQ(  0.33,   j2p_s.get_value(   ) );
           ASSERT_EQ(  0.33 + offset,   c_j2p_s.get_value(   ) );
           ASSERT_EQ(  0.44,   j2v_s.get_value(   ) );
           ASSERT_EQ(  0.55,   j1p_c.get_value(   ) );
           ASSERT_EQ(  0.66,   j1v_c.get_value(   ) );
           ASSERT_EQ(  0.77,   j2p_c.get_value(   ) );
           ASSERT_EQ(  0.88,   j2v_c.get_value(   ) );
          
           deactivate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
          }
          
          TEST_F(  TestGenericSystem,   valid_urdf_ros2_control_system_robot_with_gpio_ )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
          
           // check is hardware is started
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           configure_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           activate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
          
           ASSERT_EQ(  8u,   rm.state_interface_keys(   ).size(   ) );
           ASSERT_EQ(  6u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_output1" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_input1" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_input2" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_vacuum/vacuum" ) );
          
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_analog_IOs/analog_output1" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_vacuum/vacuum" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface gpio1_a_o1_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_output1" );
           hardware_interface::LoanedStateInterface gpio1_a_i1_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_input1" );
           hardware_interface::LoanedStateInterface gpio1_a_o2_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_input2" );
           hardware_interface::LoanedStateInterface gpio2_vac_s =
           rm.claim_state_interface(  "flange_vacuum/vacuum" );
           hardware_interface::LoanedCommandInterface gpio1_a_o1_c =
           rm.claim_command_interface(  "flange_analog_IOs/analog_output1" );
           hardware_interface::LoanedCommandInterface gpio2_vac_c =
           rm.claim_command_interface(  "flange_vacuum/vacuum" );
          
           // State interfaces without initial value are set to 0
           ASSERT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  gpio1_a_o1_c.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  gpio2_vac_c.get_value(   ) ) );
          
           // set some new values in commands
           gpio1_a_o1_c.set_value(  0.111 );
           gpio2_vac_c.set_value(  0.222 );
          
           // State values should not be changed
           ASSERT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           ASSERT_EQ(  0.111,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.222,   gpio2_vac_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           ASSERT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           ASSERT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           ASSERT_EQ(  0.111,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.222,   gpio2_vac_c.get_value(   ) );
          
           // read(   ) mirrors commands + offset to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.111,   gpio1_a_o1_s.get_value(   ) );
           ASSERT_EQ(  0.222,   gpio2_vac_s.get_value(   ) );
           ASSERT_EQ(  0.111,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.222,   gpio2_vac_c.get_value(   ) );
          
           // set some new values in commands
           gpio1_a_o1_c.set_value(  0.333 );
           gpio2_vac_c.set_value(  0.444 );
          
           // state values should not be changed
           ASSERT_EQ(  0.111,   gpio1_a_o1_s.get_value(   ) );
           ASSERT_EQ(  0.222,   gpio2_vac_s.get_value(   ) );
           ASSERT_EQ(  0.333,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.444,   gpio2_vac_c.get_value(   ) );
          
           // check other functionalities are working well
           generic_system_functional_test(  urdf );
          }
          
          TEST_F(  TestGenericSystem,   valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ )
          {
           auto urdf = ros2_control_test_assets::urdf_head +
           valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ +
           ros2_control_test_assets::urdf_tail;
           hardware_interface::ResourceManager rm(  urdf );
          
           // check is hardware is started
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           configure_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           activate_components(  rm );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["GenericSystem2dof"].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
          
           // Check interfaces
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
           ASSERT_EQ(  8u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_output1" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_input1" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_analog_IOs/analog_input2" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "flange_vacuum/vacuum" ) );
          
           ASSERT_EQ(  8u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_analog_IOs/analog_output1" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_analog_IOs/analog_input1" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_analog_IOs/analog_input2" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "flange_vacuum/vacuum" ) );
          
           // Check initial values
           hardware_interface::LoanedStateInterface gpio1_a_o1_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_output1" );
           hardware_interface::LoanedStateInterface gpio1_a_i1_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_input1" );
           hardware_interface::LoanedStateInterface gpio1_a_o2_s =
           rm.claim_state_interface(  "flange_analog_IOs/analog_input2" );
           hardware_interface::LoanedStateInterface gpio2_vac_s =
           rm.claim_state_interface(  "flange_vacuum/vacuum" );
           hardware_interface::LoanedCommandInterface gpio1_a_o1_c =
           rm.claim_command_interface(  "flange_analog_IOs/analog_output1" );
           hardware_interface::LoanedCommandInterface gpio1_a_i1_c =
           rm.claim_command_interface(  "flange_analog_IOs/analog_input1" );
           hardware_interface::LoanedCommandInterface gpio1_a_i2_c =
           rm.claim_command_interface(  "flange_analog_IOs/analog_input2" );
           hardware_interface::LoanedCommandInterface gpio2_vac_c =
           rm.claim_command_interface(  "flange_vacuum/vacuum" );
          
           EXPECT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_i1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_o2_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_o1_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_i1_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_i2_c.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio2_vac_c.get_value(   ) ) );
          
           // set some new values in commands
           gpio1_a_o1_c.set_value(  0.11 );
           gpio1_a_i1_c.set_value(  0.33 );
           gpio1_a_i2_c.set_value(  1.11 );
           gpio2_vac_c.set_value(  2.22 );
          
           // State values should not be changed
           EXPECT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_i1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_o2_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.33,   gpio1_a_i1_c.get_value(   ) );
           ASSERT_EQ(  1.11,   gpio1_a_i2_c.get_value(   ) );
           ASSERT_EQ(  2.22,   gpio2_vac_c.get_value(   ) );
          
           // write(   ) does not change values
           rm.write(  TIME,   PERIOD );
           EXPECT_TRUE(  std::isnan(  gpio1_a_o1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_i1_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio1_a_o2_s.get_value(   ) ) );
           EXPECT_TRUE(  std::isnan(  gpio2_vac_s.get_value(   ) ) );
           ASSERT_EQ(  0.11,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.33,   gpio1_a_i1_c.get_value(   ) );
           ASSERT_EQ(  1.11,   gpio1_a_i2_c.get_value(   ) );
           ASSERT_EQ(  2.22,   gpio2_vac_c.get_value(   ) );
          
           // read(   ) mirrors commands to states
           rm.read(  TIME,   PERIOD );
           ASSERT_EQ(  0.11,   gpio1_a_o1_s.get_value(   ) );
           ASSERT_EQ(  0.33,   gpio1_a_i1_s.get_value(   ) );
           ASSERT_EQ(  1.11,   gpio1_a_o2_s.get_value(   ) );
           ASSERT_EQ(  2.22,   gpio2_vac_s.get_value(   ) );
           ASSERT_EQ(  0.11,   gpio1_a_o1_c.get_value(   ) );
           ASSERT_EQ(  0.33,   gpio1_a_i1_c.get_value(   ) );
           ASSERT_EQ(  1.11,   gpio1_a_i2_c.get_value(   ) );
           ASSERT_EQ(  2.22,   gpio2_vac_c.get_value(   ) );
          }

ros2_control/hardware_interface/test/test_component_interfaces.cpp

          // Copyright 2020 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          
          #include <array>
          #include <limits>
          #include <memory>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/actuator.hpp"
          #include "hardware_interface/actuator_interface.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/sensor.hpp"
          #include "hardware_interface/sensor_interface.hpp"
          #include "hardware_interface/system.hpp"
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          // Values to send over command interface to trigger error in write and read methods
          
          namespace
          {
      41  const auto TIME = rclcpp::Time(  0 );
          const auto PERIOD = rclcpp::Duration::from_seconds(  0.01 );
          constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000;
          } // namespace
          
          using namespace ::testing; // NOLINT
          
          namespace test_components
          {
          using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
          
          class DummyActuator : public hardware_interface::ActuatorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & /*info*/ ) override
           {
           // We hardcode the info
           return CallbackReturn::SUCCESS;
           }
          
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           position_state_ = 0.0;
           velocity_state_ = 0.0;
          
           if (  recoverable_error_happened_ )
           {
           velocity_command_ = 0.0;
           }
          
           read_calls_ = 0;
           write_calls_ = 0;
          
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override
           {
           // We can read a position and a velocity
           std::vector<hardware_interface::StateInterface> state_interfaces;
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint1",   hardware_interface::HW_IF_POSITION,   &position_state_ ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint1",   hardware_interface::HW_IF_VELOCITY,   &velocity_state_ ) );
          
           return state_interfaces;
           }
          
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override
           {
           // We can command in velocity
           std::vector<hardware_interface::CommandInterface> command_interfaces;
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           "joint1",   hardware_interface::HW_IF_VELOCITY,   &velocity_command_ ) );
          
           return command_interfaces;
           }
          
           std::string get_name(   ) const override { return "DummyActuator"; }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           ++read_calls_;
           if (  read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           // no-op,   state is getting propagated within write.
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type write(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           ++write_calls_;
           if (  write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           position_state_ += velocity_command_;
           velocity_state_ = velocity_command_;
          
           return hardware_interface::return_type::OK;
           }
          
           CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           velocity_state_ = 0;
           return CallbackReturn::SUCCESS;
           }
          
           CallbackReturn on_error(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           if (  !recoverable_error_happened_ )
           {
           recoverable_error_happened_ = true;
           return CallbackReturn::SUCCESS;
           }
           else
           {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::FAILURE;
           }
          
          private:
           double position_state_ = std::numeric_limits<double>::quiet_NaN(   );
           double velocity_state_ = std::numeric_limits<double>::quiet_NaN(   );
           double velocity_command_ = 0.0;
          
           // Helper variables to initiate error on read
           unsigned int read_calls_ = 0;
           unsigned int write_calls_ = 0;
           bool recoverable_error_happened_ = false;
          };
          
          class DummySensor : public hardware_interface::SensorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & /*info*/ ) override
           {
           // We hardcode the info
           return CallbackReturn::SUCCESS;
           }
          
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           voltage_level_ = 0.0;
           read_calls_ = 0;
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override
           {
           // We can read some voltage level
           std::vector<hardware_interface::StateInterface> state_interfaces;
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  "joint1",   "voltage",   &voltage_level_ ) );
          
           return state_interfaces;
           }
          
           std::string get_name(   ) const override { return "DummySensor"; }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           ++read_calls_;
           if (  read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           // no-op,   static value
           voltage_level_ = voltage_level_hw_value_;
           return hardware_interface::return_type::OK;
           }
          
           CallbackReturn on_error(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           if (  !recoverable_error_happened_ )
           {
           recoverable_error_happened_ = true;
           return CallbackReturn::SUCCESS;
           }
           else
           {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::FAILURE;
           }
          
          private:
           double voltage_level_ = std::numeric_limits<double>::quiet_NaN(   );
           double voltage_level_hw_value_ = 0x666;
          
           // Helper variables to initiate error on read
           int read_calls_ = 0;
           bool recoverable_error_happened_ = false;
          };
          
          class DummySystem : public hardware_interface::SystemInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & /* info */ ) override
           {
           // We hardcode the info
           return CallbackReturn::SUCCESS;
           }
          
           CallbackReturn on_configure(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           for (  auto i = 0ul; i < 3; ++i )
           {
           position_state_[i] = 0.0;
           velocity_state_[i] = 0.0;
           }
           // reset command only if error is initiated
           if (  recoverable_error_happened_ )
           {
           for (  auto i = 0ul; i < 3; ++i )
           {
           velocity_command_[i] = 0.0;
           }
           }
          
           read_calls_ = 0;
           write_calls_ = 0;
          
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override
           {
           // We can read a position and a velocity
           std::vector<hardware_interface::StateInterface> state_interfaces;
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint1",   hardware_interface::HW_IF_POSITION,   &position_state_[0] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint1",   hardware_interface::HW_IF_VELOCITY,   &velocity_state_[0] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint2",   hardware_interface::HW_IF_POSITION,   &position_state_[1] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint2",   hardware_interface::HW_IF_VELOCITY,   &velocity_state_[1] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint3",   hardware_interface::HW_IF_POSITION,   &position_state_[2] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           "joint3",   hardware_interface::HW_IF_VELOCITY,   &velocity_state_[2] ) );
          
           return state_interfaces;
           }
          
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override
           {
           // We can command in velocity
           std::vector<hardware_interface::CommandInterface> command_interfaces;
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           "joint1",   hardware_interface::HW_IF_VELOCITY,   &velocity_command_[0] ) );
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           "joint2",   hardware_interface::HW_IF_VELOCITY,   &velocity_command_[1] ) );
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           "joint3",   hardware_interface::HW_IF_VELOCITY,   &velocity_command_[2] ) );
          
           return command_interfaces;
           }
          
           std::string get_name(   ) const override { return "DummySystem"; }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           ++read_calls_;
           if (  read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           // no-op,   state is getting propagated within write.
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type write(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           ++write_calls_;
           if (  write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           for (  auto i = 0; i < 3; ++i )
           {
           position_state_[i] += velocity_command_[0];
           velocity_state_[i] = velocity_command_[0];
           }
           return hardware_interface::return_type::OK;
           }
          
           CallbackReturn on_shutdown(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           for (  auto i = 0ul; i < 3; ++i )
           {
           velocity_state_[i] = 0.0;
           }
           return CallbackReturn::SUCCESS;
           }
          
           CallbackReturn on_error(  const rclcpp_lifecycle::State & /*previous_state*/ ) override
           {
           if (  !recoverable_error_happened_ )
           {
           recoverable_error_happened_ = true;
           return CallbackReturn::SUCCESS;
           }
           else
           {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::FAILURE;
           }
          
          private:
           std::array<double,   3> position_state_ = {
           std::numeric_limits<double>::quiet_NaN(   ),   std::numeric_limits<double>::quiet_NaN(   ),  
           std::numeric_limits<double>::quiet_NaN(   )};
           std::array<double,   3> velocity_state_ = {
           std::numeric_limits<double>::quiet_NaN(   ),   std::numeric_limits<double>::quiet_NaN(   ),  
           std::numeric_limits<double>::quiet_NaN(   )};
           std::array<double,   3> velocity_command_ = {0.0,   0.0,   0.0};
          
           // Helper variables to initiate error on read
           unsigned int read_calls_ = 0;
           unsigned int write_calls_ = 0;
           bool recoverable_error_happened_ = false;
          };
          
          class DummySystemPreparePerform : public hardware_interface::SystemInterface
          {
           // Override the pure virtual functions with default behavior
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & /* info */ ) override
           {
           // We hardcode the info
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override { return {}; }
          
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override
           {
           return {};
           }
          
           std::string get_name(   ) const override { return "DummySystemPreparePerform"; }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type write(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          
           // Custom prepare/perform functions
           hardware_interface::return_type prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces ) override
           {
           // Criteria to test against
           if (  start_interfaces.size(   ) != 1 )
           {
           return hardware_interface::return_type::ERROR;
           }
           if (  stop_interfaces.size(   ) != 2 )
           {
           return hardware_interface::return_type::ERROR;
           }
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces ) override
           {
           // Criteria to test against
           if (  start_interfaces.size(   ) != 1 )
           {
           return hardware_interface::return_type::ERROR;
           }
           if (  stop_interfaces.size(   ) != 2 )
           {
           return hardware_interface::return_type::ERROR;
           }
           return hardware_interface::return_type::OK;
           }
          };
          
          } // namespace test_components
          
          TEST(  TestComponentInterfaces,   dummy_actuator )
          {
           hardware_interface::Actuator actuator_hw(  std::make_unique<test_components::DummyActuator>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = actuator_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = actuator_hw.export_state_interfaces(   );
           ASSERT_EQ(  2u,   state_interfaces.size(   ) );
           EXPECT_EQ(  "joint1/position",   state_interfaces[0].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_POSITION,   state_interfaces[0].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   state_interfaces[0].get_prefix_name(   ) );
           EXPECT_EQ(  "joint1/velocity",   state_interfaces[1].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   state_interfaces[1].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   state_interfaces[1].get_prefix_name(   ) );
          
           auto command_interfaces = actuator_hw.export_command_interfaces(   );
           ASSERT_EQ(  1u,   command_interfaces.size(   ) );
           EXPECT_EQ(  "joint1/velocity",   command_interfaces[0].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   command_interfaces[0].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   command_interfaces[0].get_prefix_name(   ) );
          
           double velocity_value = 1.0;
           command_interfaces[0].set_value(  velocity_value ); // velocity
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.write(  TIME,   PERIOD ) );
          
           // Noting should change because it is UNCONFIGURED
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.read(  TIME,   PERIOD ) );
          
           ASSERT_TRUE(  std::isnan(  state_interfaces[0].get_value(   ) ) ); // position value
           ASSERT_TRUE(  std::isnan(  state_interfaces[1].get_value(   ) ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.write(  TIME,   PERIOD ) );
           }
          
           state = actuator_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::INACTIVE,   state.label(   ) );
          
           // Read and Write are working because it is INACTIVE
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  step * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  step ? velocity_value : 0,   state_interfaces[1].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
           }
          
           state = actuator_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           // Read and Write are working because it is ACTIVE
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  (  10 + step ) * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  velocity_value,   state_interfaces[1].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
           }
          
           state = actuator_hw.shutdown(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // Noting should change because it is FINALIZED
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  20 * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  0,   state_interfaces[1].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.write(  TIME,   PERIOD ) );
           }
          
           EXPECT_EQ(  
           hardware_interface::return_type::OK,   actuator_hw.prepare_command_mode_switch(  {""},   {""} ) );
           EXPECT_EQ(  
           hardware_interface::return_type::OK,   actuator_hw.perform_command_mode_switch(  {""},   {""} ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_sensor )
          {
           hardware_interface::Sensor sensor_hw(  std::make_unique<test_components::DummySensor>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = sensor_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = sensor_hw.export_state_interfaces(   );
           ASSERT_EQ(  1u,   state_interfaces.size(   ) );
           EXPECT_EQ(  "joint1/voltage",   state_interfaces[0].get_name(   ) );
           EXPECT_EQ(  "voltage",   state_interfaces[0].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   state_interfaces[0].get_prefix_name(   ) );
           EXPECT_TRUE(  std::isnan(  state_interfaces[0].get_value(   ) ) );
          
           // Not updated because is is UNCONFIGURED
           sensor_hw.read(  TIME,   PERIOD );
           EXPECT_TRUE(  std::isnan(  state_interfaces[0].get_value(   ) ) );
          
           // Updated because is is INACTIVE
           state = sensor_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::INACTIVE,   state.label(   ) );
           EXPECT_EQ(  0.0,   state_interfaces[0].get_value(   ) );
          
           // It can read now
           sensor_hw.read(  TIME,   PERIOD );
           EXPECT_EQ(  0x666,   state_interfaces[0].get_value(   ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_system )
          {
           hardware_interface::System system_hw(  std::make_unique<test_components::DummySystem>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = system_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = system_hw.export_state_interfaces(   );
           ASSERT_EQ(  6u,   state_interfaces.size(   ) );
           EXPECT_EQ(  "joint1/position",   state_interfaces[0].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_POSITION,   state_interfaces[0].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   state_interfaces[0].get_prefix_name(   ) );
           EXPECT_EQ(  "joint1/velocity",   state_interfaces[1].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   state_interfaces[1].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   state_interfaces[1].get_prefix_name(   ) );
           EXPECT_EQ(  "joint2/position",   state_interfaces[2].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_POSITION,   state_interfaces[2].get_interface_name(   ) );
           EXPECT_EQ(  "joint2",   state_interfaces[2].get_prefix_name(   ) );
           EXPECT_EQ(  "joint2/velocity",   state_interfaces[3].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   state_interfaces[3].get_interface_name(   ) );
           EXPECT_EQ(  "joint2",   state_interfaces[3].get_prefix_name(   ) );
           EXPECT_EQ(  "joint3/position",   state_interfaces[4].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_POSITION,   state_interfaces[4].get_interface_name(   ) );
           EXPECT_EQ(  "joint3",   state_interfaces[4].get_prefix_name(   ) );
           EXPECT_EQ(  "joint3/velocity",   state_interfaces[5].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   state_interfaces[5].get_interface_name(   ) );
           EXPECT_EQ(  "joint3",   state_interfaces[5].get_prefix_name(   ) );
          
           auto command_interfaces = system_hw.export_command_interfaces(   );
           ASSERT_EQ(  3u,   command_interfaces.size(   ) );
           EXPECT_EQ(  "joint1/velocity",   command_interfaces[0].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   command_interfaces[0].get_interface_name(   ) );
           EXPECT_EQ(  "joint1",   command_interfaces[0].get_prefix_name(   ) );
           EXPECT_EQ(  "joint2/velocity",   command_interfaces[1].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   command_interfaces[1].get_interface_name(   ) );
           EXPECT_EQ(  "joint2",   command_interfaces[1].get_prefix_name(   ) );
           EXPECT_EQ(  "joint3/velocity",   command_interfaces[2].get_name(   ) );
           EXPECT_EQ(  hardware_interface::HW_IF_VELOCITY,   command_interfaces[2].get_interface_name(   ) );
           EXPECT_EQ(  "joint3",   command_interfaces[2].get_prefix_name(   ) );
          
           double velocity_value = 1.0;
           command_interfaces[0].set_value(  velocity_value ); // velocity
           command_interfaces[1].set_value(  velocity_value ); // velocity
           command_interfaces[2].set_value(  velocity_value ); // velocity
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.write(  TIME,   PERIOD ) );
          
           // Noting should change because it is UNCONFIGURED
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.read(  TIME,   PERIOD ) );
          
           ASSERT_TRUE(  std::isnan(  state_interfaces[0].get_value(   ) ) ); // position value
           ASSERT_TRUE(  std::isnan(  state_interfaces[1].get_value(   ) ) ); // velocity
           ASSERT_TRUE(  std::isnan(  state_interfaces[2].get_value(   ) ) ); // position value
           ASSERT_TRUE(  std::isnan(  state_interfaces[3].get_value(   ) ) ); // velocity
           ASSERT_TRUE(  std::isnan(  state_interfaces[4].get_value(   ) ) ); // position value
           ASSERT_TRUE(  std::isnan(  state_interfaces[5].get_value(   ) ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.write(  TIME,   PERIOD ) );
           }
          
           state = system_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::INACTIVE,   state.label(   ) );
          
           // Read and Write are working because it is INACTIVE
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  step * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  step ? velocity_value : 0,   state_interfaces[1].get_value(   ) ); // velocity
           EXPECT_EQ(  step * velocity_value,   state_interfaces[2].get_value(   ) ); // position value
           EXPECT_EQ(  step ? velocity_value : 0,   state_interfaces[3].get_value(   ) ); // velocity
           EXPECT_EQ(  step * velocity_value,   state_interfaces[4].get_value(   ) ); // position value
           EXPECT_EQ(  step ? velocity_value : 0,   state_interfaces[5].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
           }
          
           state = system_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           // Read and Write are working because it is ACTIVE
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  (  10 + step ) * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  velocity_value,   state_interfaces[1].get_value(   ) ); // velocity
           EXPECT_EQ(  (  10 + step ) * velocity_value,   state_interfaces[2].get_value(   ) ); // position value
           EXPECT_EQ(  velocity_value,   state_interfaces[3].get_value(   ) ); // velocity
           EXPECT_EQ(  (  10 + step ) * velocity_value,   state_interfaces[4].get_value(   ) ); // position value
           EXPECT_EQ(  velocity_value,   state_interfaces[5].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
           }
          
           state = system_hw.shutdown(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // Noting should change because it is FINALIZED
           for (  auto step = 0u; step < 10; ++step )
           {
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.read(  TIME,   PERIOD ) );
          
           EXPECT_EQ(  20 * velocity_value,   state_interfaces[0].get_value(   ) ); // position value
           EXPECT_EQ(  0.0,   state_interfaces[1].get_value(   ) ); // velocity
           EXPECT_EQ(  20 * velocity_value,   state_interfaces[2].get_value(   ) ); // position value
           EXPECT_EQ(  0.0,   state_interfaces[3].get_value(   ) ); // velocity
           EXPECT_EQ(  20 * velocity_value,   state_interfaces[4].get_value(   ) ); // position value
           EXPECT_EQ(  0.0,   state_interfaces[5].get_value(   ) ); // velocity
          
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.write(  TIME,   PERIOD ) );
           }
          
           EXPECT_EQ(  hardware_interface::return_type::OK,   system_hw.prepare_command_mode_switch(  {},   {} ) );
           EXPECT_EQ(  hardware_interface::return_type::OK,   system_hw.perform_command_mode_switch(  {},   {} ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_command_mode_system )
          {
           hardware_interface::System system_hw(  
           std::make_unique<test_components::DummySystemPreparePerform>(   ) );
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = system_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           std::vector<std::string> one_key = {"joint1/position"};
           std::vector<std::string> two_keys = {"joint1/position",   "joint1/velocity"};
          
           // Only calls with (  one_key,   two_keys ) should return OK
           EXPECT_EQ(  
           hardware_interface::return_type::ERROR,  
           system_hw.prepare_command_mode_switch(  one_key,   one_key ) );
           EXPECT_EQ(  
           hardware_interface::return_type::ERROR,  
           system_hw.perform_command_mode_switch(  one_key,   one_key ) );
           EXPECT_EQ(  
           hardware_interface::return_type::OK,   system_hw.prepare_command_mode_switch(  one_key,   two_keys ) );
           EXPECT_EQ(  
           hardware_interface::return_type::OK,   system_hw.perform_command_mode_switch(  one_key,   two_keys ) );
           EXPECT_EQ(  
           hardware_interface::return_type::ERROR,  
           system_hw.prepare_command_mode_switch(  two_keys,   one_key ) );
           EXPECT_EQ(  
           hardware_interface::return_type::ERROR,  
           system_hw.perform_command_mode_switch(  two_keys,   one_key ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_actuator_read_error_behavior )
          {
           hardware_interface::Actuator actuator_hw(  std::make_unique<test_components::DummyActuator>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = actuator_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = actuator_hw.export_state_interfaces(   );
           auto command_interfaces = actuator_hw.export_command_interfaces(   );
           state = actuator_hw.configure(   );
           state = actuator_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is first time therefore recoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.read(  TIME,   PERIOD ) );
          
           state = actuator_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           // activate again and expect reset values
           state = actuator_hw.configure(   );
           EXPECT_EQ(  state_interfaces[0].get_value(   ),   0.0 );
           EXPECT_EQ(  command_interfaces[0].get_value(   ),   0.0 );
          
           state = actuator_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is the second time therefore unrecoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.read(  TIME,   PERIOD ) );
          
           state = actuator_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // can not change state anymore
           state = actuator_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_actuator_write_error_behavior )
          {
           hardware_interface::Actuator actuator_hw(  std::make_unique<test_components::DummyActuator>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = actuator_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = actuator_hw.export_state_interfaces(   );
           auto command_interfaces = actuator_hw.export_command_interfaces(   );
           state = actuator_hw.configure(   );
           state = actuator_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is first time therefore recoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.write(  TIME,   PERIOD ) );
          
           state = actuator_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           // activate again and expect reset values
           state = actuator_hw.configure(   );
           EXPECT_EQ(  state_interfaces[0].get_value(   ),   0.0 );
           EXPECT_EQ(  command_interfaces[0].get_value(   ),   0.0 );
          
           state = actuator_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is the second time therefore unrecoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   actuator_hw.write(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   actuator_hw.write(  TIME,   PERIOD ) );
          
           state = actuator_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // can not change state anymore
           state = actuator_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_sensor_read_error_behavior )
          {
           hardware_interface::Sensor sensor_hw(  std::make_unique<test_components::DummySensor>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = sensor_hw.initialize(  mock_hw_info );
          
           auto state_interfaces = sensor_hw.export_state_interfaces(   );
           // Updated because is is INACTIVE
           state = sensor_hw.configure(   );
           state = sensor_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   sensor_hw.read(  TIME,   PERIOD ) );
          
           // Initiate recoverable error - call read 99 times OK and on 100-time will return error
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   sensor_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   sensor_hw.read(  TIME,   PERIOD ) );
          
           state = sensor_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           // activate again and expect reset values
           state = sensor_hw.configure(   );
           EXPECT_EQ(  state_interfaces[0].get_value(   ),   0.0 );
          
           state = sensor_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error
           for (  auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   sensor_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   sensor_hw.read(  TIME,   PERIOD ) );
          
           state = sensor_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // can not change state anymore
           state = sensor_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_system_read_error_behavior )
          {
           hardware_interface::System system_hw(  std::make_unique<test_components::DummySystem>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = system_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = system_hw.export_state_interfaces(   );
           auto command_interfaces = system_hw.export_command_interfaces(   );
           state = system_hw.configure(   );
           state = system_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is first time therefore recoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.read(  TIME,   PERIOD ) );
          
           state = system_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           // activate again and expect reset values
           state = system_hw.configure(   );
           for (  auto index = 0ul; index < 6; ++index )
           {
           EXPECT_EQ(  state_interfaces[index].get_value(   ),   0.0 );
           }
           for (  auto index = 0ul; index < 3; ++index )
           {
           EXPECT_EQ(  command_interfaces[index].get_value(   ),   0.0 );
           }
           state = system_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is the second time therefore unrecoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.read(  TIME,   PERIOD ) );
          
           state = system_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // can not change state anymore
           state = system_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          }
          
          TEST(  TestComponentInterfaces,   dummy_system_write_error_behavior )
          {
           hardware_interface::System system_hw(  std::make_unique<test_components::DummySystem>(   ) );
          
           hardware_interface::HardwareInfo mock_hw_info{};
           auto state = system_hw.initialize(  mock_hw_info );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           auto state_interfaces = system_hw.export_state_interfaces(   );
           auto command_interfaces = system_hw.export_command_interfaces(   );
           state = system_hw.configure(   );
           state = system_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is first time therefore recoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.write(  TIME,   PERIOD ) );
          
           state = system_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::UNCONFIGURED,   state.label(   ) );
          
           // activate again and expect reset values
           state = system_hw.configure(   );
           for (  auto index = 0ul; index < 6; ++index )
           {
           EXPECT_EQ(  state_interfaces[index].get_value(   ),   0.0 );
           }
           for (  auto index = 0ul; index < 3; ++index )
           {
           EXPECT_EQ(  command_interfaces[index].get_value(   ),   0.0 );
           }
           state = system_hw.activate(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::ACTIVE,   state.label(   ) );
          
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.read(  TIME,   PERIOD ) );
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
          
           // Initiate error on write (  this is the second time therefore unrecoverable )
           for (  auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
           {
           ASSERT_EQ(  hardware_interface::return_type::OK,   system_hw.write(  TIME,   PERIOD ) );
           }
           ASSERT_EQ(  hardware_interface::return_type::ERROR,   system_hw.write(  TIME,   PERIOD ) );
          
           state = system_hw.get_state(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          
           // can not change state anymore
           state = system_hw.configure(   );
           EXPECT_EQ(  lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,   state.id(   ) );
           EXPECT_EQ(  hardware_interface::lifecycle_state_names::FINALIZED,   state.label(   ) );
          }

ros2_control/hardware_interface/test/test_component_parser.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <string>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "ros2_control_test_assets/components_urdfs.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
          #ifdef _WIN32
          #define M_PI 3.1415926535897932384626433832795
          #endif
          
          using namespace ::testing; // NOLINT
          
      29  class TestComponentParser : public Test
          {
          protected:
           void SetUp(   ) override {}
          };
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using hardware_interface::parse_control_resources_from_urdf;
          
          TEST_F(  TestComponentParser,   empty_string_throws_error )
          {
           ASSERT_THROW(  parse_control_resources_from_urdf(  "" ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   empty_urdf_throws_error )
          {
           const std::string empty_urdf =
           "<?xml version=\"1.0\"?><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot>";
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  empty_urdf ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   string_robot_not_root_throws_error )
          {
           const std::string broken_xml_string =
           R"(  
           <?xml version=\"1.0\"?><ros2_control name=\"robot\">><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot></ros2_control>
            )";
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_xml_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   invalid_child_throws_error )
          {
           const std::string broken_urdf_string =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf_ros2_control_invalid_child +
           ros2_control_test_assets::urdf_tail;
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_urdf_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   missing_attribute_throws_error )
          {
           const std::string broken_urdf_string =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf_ros2_control_missing_attribute +
           ros2_control_test_assets::urdf_tail;
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_urdf_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   parameter_missing_name_throws_error )
          {
           const std::string broken_urdf_string =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf_ros2_control_parameter_missing_name +
           ros2_control_test_assets::urdf_tail;
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_urdf_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   component_interface_type_empty_throws_error )
          {
           const std::string broken_urdf_string =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf_ros2_control_component_interface_type_empty +
           ros2_control_test_assets::urdf_tail;
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_urdf_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   parameter_empty_throws_error )
          {
           const std::string broken_urdf_string =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty +
           ros2_control_test_assets::urdf_tail;
          
           ASSERT_THROW(  parse_control_resources_from_urdf(  broken_urdf_string ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_one_interface )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           const auto hardware_info = control_hardware.front(   );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemPositionOnly" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,  
           "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_POSITION );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].min,   "-1" );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].max,   "1" );
           ASSERT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].name,   HW_IF_POSITION );
          
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[1].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[1].command_interfaces[0].name,   HW_IF_POSITION );
           EXPECT_EQ(  hardware_info.joints[1].command_interfaces[0].min,   "-1" );
           EXPECT_EQ(  hardware_info.joints[1].command_interfaces[0].max,   "1" );
           ASSERT_THAT(  hardware_info.joints[1].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[1].state_interfaces[0].name,   HW_IF_POSITION );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_multi_interface )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_multi_interface +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           const auto hardware_info = control_hardware.front(   );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemMultiInterface" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,  
           "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "2" );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  3 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_POSITION );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].initial_value,   "1.2" );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[1].initial_value,   "3.4" );
           ASSERT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  3 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[1].name,   HW_IF_VELOCITY );
          
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[1].command_interfaces,   SizeIs(  1 ) );
           ASSERT_THAT(  hardware_info.joints[1].state_interfaces,   SizeIs(  3 ) );
           EXPECT_EQ(  hardware_info.joints[1].state_interfaces[2].name,   HW_IF_EFFORT );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_robot_with_sensor )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_sensor +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           const auto hardware_info = control_hardware.front(   );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemWithSensor" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/RRBotSystemWithSensorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
          
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  1 ) );
          
           EXPECT_EQ(  hardware_info.sensors[0].name,   "tcp_fts_sensor" );
           EXPECT_EQ(  hardware_info.sensors[0].type,   "sensor" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces,   SizeIs(  6 ) );
           EXPECT_THAT(  hardware_info.sensors[0].command_interfaces,   IsEmpty(   ) );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[0].name,   "fx" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[1].name,   "fy" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[2].name,   "fz" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[3].name,   "tx" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[4].name,   "ty" );
           EXPECT_THAT(  hardware_info.sensors[0].state_interfaces[5].name,   "tz" );
          
           ASSERT_THAT(  hardware_info.sensors[0].parameters,   SizeIs(  3 ) );
           EXPECT_EQ(  hardware_info.sensors[0].parameters.at(  "frame_id" ),   "kuka_tcp" );
           EXPECT_EQ(  hardware_info.sensors[0].parameters.at(  "lower_limits" ),   "-100" );
           EXPECT_EQ(  hardware_info.sensors[0].parameters.at(  "upper_limits" ),   "100" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_robot_with_external_sensor )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_external_sensor +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  2 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemPositionOnlyWithExternalSensor" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,  
           "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
          
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  0 ) );
          
           hardware_info = control_hardware.at(  1 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotForceTorqueSensor2D" );
           EXPECT_EQ(  hardware_info.type,   "sensor" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "0.43" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.sensors[0].name,   "tcp_fts_sensor" );
           EXPECT_EQ(  hardware_info.sensors[0].type,   "sensor" );
           EXPECT_EQ(  hardware_info.sensors[0].parameters.at(  "frame_id" ),   "kuka_tcp" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_actuator_modular_robot )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_actuator_modular_robot +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  2 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularJoint1" );
           EXPECT_EQ(  hardware_info.type,   "actuator" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/PositionActuatorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "1.23" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
          
           hardware_info = control_hardware.at(  1 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularJoint2" );
           EXPECT_EQ(  hardware_info.type,   "actuator" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/PositionActuatorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "3" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_actuator_modular_robot_with_sensors )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_actuator_modular_robot_sensors +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  4 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularJoint1" );
           EXPECT_EQ(  hardware_info.type,   "actuator" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/VelocityActuatorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "1.23" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_VELOCITY );
          
           ASSERT_THAT(  hardware_info.transmissions,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.transmissions[0].name,   "transmission1" );
           EXPECT_EQ(  hardware_info.transmissions[0].type,   "transmission_interface/SimpleTansmission" );
           ASSERT_THAT(  hardware_info.transmissions[0].joints,   SizeIs(  1 ) );
           EXPECT_THAT(  hardware_info.transmissions[0].joints[0].name,   "joint1" );
           EXPECT_THAT(  
           hardware_info.transmissions[0].joints[0].mechanical_reduction,   DoubleNear(  1024.0 / M_PI,   0.01 ) );
           ASSERT_THAT(  hardware_info.transmissions[0].actuators,   SizeIs(  1 ) );
           EXPECT_THAT(  hardware_info.transmissions[0].actuators[0].name,   "actuator1" );
          
           hardware_info = control_hardware.at(  1 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularJoint2" );
           EXPECT_EQ(  hardware_info.type,   "actuator" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/VelocityActuatorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "3" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_VELOCITY );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].min,   "-1" );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].max,   "1" );
          
           hardware_info = control_hardware.at(  2 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularPositionSensorJoint1" );
           EXPECT_EQ(  hardware_info.type,   "sensor" );
           EXPECT_EQ(  hardware_info.hardware_class_type,   "ros2_control_demo_hardware/PositionSensorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  0 ) );
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   IsEmpty(   ) );
           ASSERT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].name,   HW_IF_POSITION );
          
           hardware_info = control_hardware.at(  3 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularPositionSensorJoint2" );
           EXPECT_EQ(  hardware_info.type,   "sensor" );
           EXPECT_EQ(  hardware_info.hardware_class_type,   "ros2_control_demo_hardware/PositionSensorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  0 ) );
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   IsEmpty(   ) );
           ASSERT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].name,   HW_IF_POSITION );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_multi_joints_transmission )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotModularWrist" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/ActuatorHardwareMultiDOF" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "1.23" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
          
           ASSERT_THAT(  hardware_info.transmissions,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.transmissions[0].name,   "transmission1" );
           EXPECT_EQ(  hardware_info.transmissions[0].type,   "transmission_interface/DifferentialTransmission" );
           ASSERT_THAT(  hardware_info.transmissions[0].joints,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[0].role,   "joint1" );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[0].mechanical_reduction,   10.0 );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[0].offset,   0.5 );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[1].role,   "joint2" );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[1].mechanical_reduction,   50.0 );
           EXPECT_EQ(  hardware_info.transmissions[0].joints[1].offset,   0.0 );
          
           ASSERT_THAT(  hardware_info.transmissions[0].actuators,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.transmissions[0].actuators[0].name,   "joint1_motor" );
           EXPECT_EQ(  hardware_info.transmissions[0].actuators[0].role,   "actuator1" );
           EXPECT_EQ(  hardware_info.transmissions[0].actuators[1].name,   "joint2_motor" );
           EXPECT_EQ(  hardware_info.transmissions[0].actuators[1].role,   "actuator2" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_sensor_only )
          {
           std::string urdf_to_test = std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_sensor_only +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "CameraWithIMU" );
           EXPECT_EQ(  hardware_info.type,   "sensor" );
           EXPECT_EQ(  hardware_info.hardware_class_type,   "ros2_control_demo_hardware/CameraWithIMUSensor" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_read_for_sec" ),   "2" );
          
           ASSERT_THAT(  hardware_info.sensors,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.sensors[0].name,   "sensor1" );
           EXPECT_EQ(  hardware_info.sensors[0].type,   "sensor" );
           ASSERT_THAT(  hardware_info.sensors[0].state_interfaces,   SizeIs(  3 ) );
           EXPECT_EQ(  hardware_info.sensors[0].state_interfaces[0].name,   "roll" );
           EXPECT_EQ(  hardware_info.sensors[0].state_interfaces[1].name,   "pitch" );
           EXPECT_EQ(  hardware_info.sensors[0].state_interfaces[2].name,   "yaw" );
          
           EXPECT_EQ(  hardware_info.sensors[1].name,   "sensor2" );
           EXPECT_EQ(  hardware_info.sensors[1].type,   "sensor" );
           ASSERT_THAT(  hardware_info.sensors[1].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.sensors[1].state_interfaces[0].name,   "image" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_actuator_only )
          {
           std::string urdf_to_test = std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_actuator_only +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           auto hardware_info = control_hardware.at(  0 );
          
           EXPECT_EQ(  hardware_info.name,   "ActuatorModularJoint1" );
           EXPECT_EQ(  hardware_info.type,   "actuator" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/VelocityActuatorHardware" );
           ASSERT_THAT(  hardware_info.hardware_parameters,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.hardware_parameters.at(  "example_param_write_for_sec" ),   "1.13" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           ASSERT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_VELOCITY );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].min,   "-1" );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].max,   "1" );
           ASSERT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].name,   HW_IF_VELOCITY );
          
           ASSERT_THAT(  hardware_info.transmissions,   SizeIs(  1 ) );
           const auto transmission = hardware_info.transmissions[0];
           EXPECT_EQ(  transmission.name,   "transmission1" );
           EXPECT_EQ(  transmission.type,   "transmission_interface/RotationToLinerTansmission" );
           EXPECT_THAT(  transmission.joints,   SizeIs(  1 ) );
           const auto joint = transmission.joints[0];
           EXPECT_EQ(  joint.name,   "joint1" );
           EXPECT_EQ(  joint.role,   "joint1" );
           EXPECT_THAT(  joint.interfaces,   ElementsAre(  "velocity" ) );
           EXPECT_THAT(  joint.mechanical_reduction,   DoubleEq(  325.949 ) );
           EXPECT_THAT(  joint.offset,   DoubleEq(  0.0 ) );
           EXPECT_THAT(  transmission.actuators,   SizeIs(  1 ) );
           const auto actuator = transmission.actuators[0];
           EXPECT_EQ(  actuator.name,   "actuator1" );
           EXPECT_EQ(  actuator.role,   "actuator1" );
           EXPECT_THAT(  actuator.interfaces,   ContainerEq(  joint.interfaces ) );
           EXPECT_THAT(  actuator.offset,   DoubleEq(  0.0 ) );
           ASSERT_THAT(  transmission.parameters,   SizeIs(  1 ) );
           EXPECT_EQ(  transmission.parameters.at(  "additional_special_parameter" ),   "1337" );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_robot_with_gpio )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           auto hardware_info = control_hardware.front(   );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemWithGPIO" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
          
           EXPECT_EQ(  hardware_info.joints[1].name,   "joint2" );
           EXPECT_EQ(  hardware_info.joints[1].type,   "joint" );
          
           ASSERT_THAT(  hardware_info.gpios,   SizeIs(  2 ) );
          
           EXPECT_EQ(  hardware_info.gpios[0].name,   "flange_analog_IOs" );
           EXPECT_EQ(  hardware_info.gpios[0].type,   "gpio" );
           EXPECT_THAT(  hardware_info.gpios[0].state_interfaces,   SizeIs(  3 ) );
           EXPECT_THAT(  hardware_info.gpios[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[0].name,   "analog_output1" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[1].name,   "analog_input1" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[2].name,   "analog_input2" );
          
           EXPECT_EQ(  hardware_info.gpios[1].name,   "flange_vacuum" );
           EXPECT_EQ(  hardware_info.gpios[1].type,   "gpio" );
           EXPECT_THAT(  hardware_info.gpios[1].state_interfaces,   SizeIs(  1 ) );
           EXPECT_THAT(  hardware_info.gpios[1].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.gpios[1].state_interfaces[0].name,   "vacuum" );
           EXPECT_EQ(  hardware_info.gpios[1].command_interfaces[0].name,   "vacuum" );
          
           EXPECT_THAT(  hardware_info.transmissions,   IsEmpty(   ) );
          }
          
          TEST_F(  TestComponentParser,   successfully_parse_valid_urdf_system_with_size_and_data_type )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_size_and_data_type +
           ros2_control_test_assets::urdf_tail;
           const auto control_hardware = parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  control_hardware,   SizeIs(  1 ) );
           auto hardware_info = control_hardware.front(   );
          
           EXPECT_EQ(  hardware_info.name,   "RRBotSystemWithSizeAndDataType" );
           EXPECT_EQ(  hardware_info.type,   "system" );
           EXPECT_EQ(  
           hardware_info.hardware_class_type,   "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType" );
          
           ASSERT_THAT(  hardware_info.joints,   SizeIs(  1 ) );
          
           EXPECT_EQ(  hardware_info.joints[0].name,   "joint1" );
           EXPECT_EQ(  hardware_info.joints[0].type,   "joint" );
           EXPECT_THAT(  hardware_info.joints[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].name,   HW_IF_POSITION );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].data_type,   "double" );
           EXPECT_EQ(  hardware_info.joints[0].command_interfaces[0].size,   1 );
           EXPECT_THAT(  hardware_info.joints[0].state_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].name,   HW_IF_POSITION );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].data_type,   "double" );
           EXPECT_EQ(  hardware_info.joints[0].state_interfaces[0].size,   1 );
          
           ASSERT_THAT(  hardware_info.gpios,   SizeIs(  1 ) );
          
           EXPECT_EQ(  hardware_info.gpios[0].name,   "flange_IOS" );
           EXPECT_EQ(  hardware_info.gpios[0].type,   "gpio" );
           EXPECT_THAT(  hardware_info.gpios[0].command_interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  hardware_info.gpios[0].command_interfaces[0].name,   "digital_output" );
           EXPECT_EQ(  hardware_info.gpios[0].command_interfaces[0].data_type,   "bool" );
           EXPECT_EQ(  hardware_info.gpios[0].command_interfaces[0].size,   2 );
           EXPECT_THAT(  hardware_info.gpios[0].state_interfaces,   SizeIs(  2 ) );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[0].name,   "analog_input" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[0].data_type,   "double" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[0].size,   3 );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[1].name,   "image" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[1].data_type,   "cv::Mat" );
           EXPECT_EQ(  hardware_info.gpios[0].state_interfaces[1].size,   1 );
          }
          
          TEST_F(  TestComponentParser,   negative_size_throws_error )
          {
           std::string urdf_to_test = std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size +
           ros2_control_test_assets::urdf_tail;
           ASSERT_THROW(  parse_control_resources_from_urdf(  urdf_to_test ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   noninteger_size_throws_error )
          {
           std::string urdf_to_test = std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size2 +
           ros2_control_test_assets::urdf_tail;
           ASSERT_THROW(  parse_control_resources_from_urdf(  urdf_to_test ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   transmission_and_component_joint_mismatch_throws_error )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf2_hw_transmission_joint_mismatch +
           ros2_control_test_assets::urdf_tail;
           ASSERT_THROW(  parse_control_resources_from_urdf(  urdf_to_test ),   std::runtime_error );
          }
          
          TEST_F(  TestComponentParser,   transmission_given_too_many_joints_throws_error )
          {
           std::string urdf_to_test =
           std::string(  ros2_control_test_assets::urdf_head ) +
           ros2_control_test_assets::invalid_urdf2_transmission_given_too_many_joints +
           ros2_control_test_assets::urdf_tail;
           ASSERT_THROW(  parse_control_resources_from_urdf(  urdf_to_test ),   std::runtime_error );
          }

ros2_control/hardware_interface/test/test_components/test_actuator.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/actuator_interface.hpp"
          
          using hardware_interface::ActuatorInterface;
          using hardware_interface::CommandInterface;
          using hardware_interface::return_type;
          using hardware_interface::StateInterface;
          
      25  class TestActuator : public ActuatorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & info ) override
           {
           if (  ActuatorInterface::on_init(  info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           /*
           * a hardware can optional prove for incorrect info here.
           *
           * // can only control one joint
           * if (  info_.joints.size(   ) != 1 ) {return CallbackReturn::ERROR;}
           * // can only control in position
           * if (  info_.joints[0].command_interfaces.size(   ) != 1 ) {return CallbackReturn::ERROR;}
           * // can only give feedback state for position and velocity
           * if (  info_.joints[0].state_interfaces.size(   ) != 2 ) {return CallbackReturn::ERROR;}
           */
          
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[0].name,   info_.joints[0].state_interfaces[0].name,   &position_state_ ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[0].name,   info_.joints[0].state_interfaces[1].name,   &velocity_state_ ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  info_.joints[0].name,   "some_unlisted_interface",   nullptr ) );
          
           return state_interfaces;
           }
          
           std::vector<CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<CommandInterface> command_interfaces;
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[0].name,   info_.joints[0].command_interfaces[0].name,   &velocity_command_ ) );
          
           if (  info_.joints[0].command_interfaces.size(   ) > 1 )
           {
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[0].name,   info_.joints[0].command_interfaces[1].name,   &max_velocity_command_ ) );
           }
          
           return command_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           // The next line is for the testing purposes. We need value to be changed to be sure that
           // the feedback from hardware to controllers in the chain is working as it should.
           // This makes value checks clearer and confirms there is no "state = command" line or some
           // other mixture of interfaces somewhere in the test stack.
           velocity_state_ = velocity_command_ / 2;
           return return_type::OK;
           }
          
           return_type write(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
          private:
           double position_state_ = 0.0;
           double velocity_state_ = 0.0;
           double velocity_command_ = 0.0;
           double max_velocity_command_ = 0.0;
          };
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  TestActuator,   hardware_interface::ActuatorInterface )

ros2_control/hardware_interface/test/test_components/test_sensor.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/sensor_interface.hpp"
          
          using hardware_interface::return_type;
          using hardware_interface::SensorInterface;
          using hardware_interface::StateInterface;
          
      24  class TestSensor : public SensorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & info ) override
           {
           if (  SensorInterface::on_init(  info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
           // can only give feedback state for velocity
           if (  info_.sensors[0].state_interfaces.size(   ) != 1 )
           {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.sensors[0].name,   info_.sensors[0].state_interfaces[0].name,   &velocity_state_ ) );
          
           return state_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
          private:
           double velocity_state_ = 0.0;
          };
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  TestSensor,   hardware_interface::SensorInterface )

ros2_control/hardware_interface/test/test_components/test_system.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <array>
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::return_type;
          using hardware_interface::StateInterface;
          using hardware_interface::SystemInterface;
          
      27  class TestSystem : public SystemInterface
          {
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); ++i )
           {
           if (  info_.joints[i].name != "configuration" )
           {
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_POSITION,   &position_state_[i] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_VELOCITY,   &velocity_state_[i] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_ACCELERATION,   &acceleration_state_[i] ) );
           }
           }
          
           if (  info_.joints.size(   ) > 2 )
           {
           // Add configuration/max_tcp_jerk interface
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[2].name,   info_.joints[2].state_interfaces[0].name,   &configuration_state_ ) );
           }
          
           return state_interfaces;
           }
          
           std::vector<CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<CommandInterface> command_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); ++i )
           {
           if (  info_.joints[i].name != "configuration" )
           {
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_VELOCITY,   &velocity_command_[i] ) );
           }
           }
           // Add max_acceleration command interface
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[0].name,   info_.joints[0].command_interfaces[1].name,  
           &max_acceleration_command_ ) );
          
           if (  info_.joints.size(   ) > 2 )
           {
           // Add configuration/max_tcp_jerk interface
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[2].name,   info_.joints[2].command_interfaces[0].name,   &configuration_command_ ) );
           }
          
           return command_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
           return_type write(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
          private:
           std::array<double,   2> velocity_command_ = {0.0,   0.0};
           std::array<double,   2> position_state_ = {0.0,   0.0};
           std::array<double,   2> velocity_state_ = {0.0,   0.0};
           std::array<double,   2> acceleration_state_ = {0.0,   0.0};
           double max_acceleration_command_ = 0.0;
           double configuration_state_ = 0.0;
           double configuration_command_ = 0.0;
          };
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  TestSystem,   hardware_interface::SystemInterface )

ros2_control/hardware_interface/test/test_handle.cpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include "hardware_interface/handle.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::StateInterface;
          
          namespace
          {
      23  constexpr auto JOINT_NAME = "joint_1";
          constexpr auto FOO_INTERFACE = "FooInterface";
          } // namespace
          
          TEST(  TestHandle,   command_interface )
          {
           double value = 1.337;
           CommandInterface interface{JOINT_NAME,   FOO_INTERFACE,   &value};
           EXPECT_DOUBLE_EQ(  interface.get_value(   ),   value );
           EXPECT_NO_THROW(  interface.set_value(  0.0 ) );
           EXPECT_DOUBLE_EQ(  interface.get_value(   ),   0.0 );
          }
          
          TEST(  TestHandle,   state_interface )
          {
           double value = 1.337;
           StateInterface interface{JOINT_NAME,   FOO_INTERFACE,   &value};
           EXPECT_DOUBLE_EQ(  interface.get_value(   ),   value );
           // interface.set_value(  5 ); compiler error,   no set_value function
          }
          
          TEST(  TestHandle,   name_getters_work )
          {
           StateInterface handle{JOINT_NAME,   FOO_INTERFACE};
           EXPECT_EQ(  handle.get_name(   ),   std::string(  JOINT_NAME ) + "/" + std::string(  FOO_INTERFACE ) );
           EXPECT_EQ(  handle.get_interface_name(   ),   FOO_INTERFACE );
           EXPECT_EQ(  handle.get_prefix_name(   ),   JOINT_NAME );
          }
          
          TEST(  TestHandle,   value_methods_throw_for_nullptr )
          {
           CommandInterface handle{JOINT_NAME,   FOO_INTERFACE};
           EXPECT_ANY_THROW(  handle.get_value(   ) );
           EXPECT_ANY_THROW(  handle.set_value(  0.0 ) );
          }
          
          TEST(  TestHandle,   value_methods_work_on_non_nullptr )
          {
           double value = 1.337;
           CommandInterface handle{JOINT_NAME,   FOO_INTERFACE,   &value};
           EXPECT_DOUBLE_EQ(  handle.get_value(   ),   value );
           EXPECT_NO_THROW(  handle.set_value(  0.0 ) );
           EXPECT_DOUBLE_EQ(  handle.get_value(   ),   0.0 );
          }

ros2_control/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <algorithm>
          #include <cmath>
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/sensor_interface.hpp"
          
          using hardware_interface::return_type;
          using hardware_interface::SensorInterface;
          using hardware_interface::StateInterface;
          
          namespace test_hardware_components
          {
      28  class TestForceTorqueSensor : public SensorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & sensor_info ) override
           {
           if (  SensorInterface::on_init(  sensor_info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           const auto & state_interfaces = info_.sensors[0].state_interfaces;
      38   if (  state_interfaces.size(   ) != 6 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const auto & ft_key : {"fx",   "fy",   "fz",   "tx",   "ty",   "tz"} )
           {
           if (  
           std::find_if(  
           state_interfaces.begin(   ),   state_interfaces.end(   ),  
           [&ft_key](  const auto & interface_info )
           { return interface_info.name == ft_key; } ) == state_interfaces.end(   ) )
           {
           return CallbackReturn::ERROR;
           }
           }
          
           fprintf(  stderr,   "TestForceTorqueSensor configured successfully.\n" );
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
          
           const auto & sensor_name = info_.sensors[0].name;
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "fx",   &values_.fx ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "fy",   &values_.fy ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "fz",   &values_.fz ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "tx",   &values_.tx ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "ty",   &values_.ty ) );
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  sensor_name,   "tz",   &values_.tz ) );
          
           return state_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           values_.fx = fmod(  (  values_.fx + 1.0 ),   10 );
           values_.fy = fmod(  (  values_.fy + 1.0 ),   10 );
           values_.fz = fmod(  (  values_.fz + 1.0 ),   10 );
           values_.tx = fmod(  (  values_.tx + 1.0 ),   10 );
           values_.ty = fmod(  (  values_.ty + 1.0 ),   10 );
           values_.tz = fmod(  (  values_.tz + 1.0 ),   10 );
           return return_type::OK;
           }
          
          private:
           struct FTValues
           {
           double fx = 0.0;
           double fy = 0.0;
           double fz = 0.0;
           double tx = 0.0;
           double ty = 0.0;
           double tz = 0.0;
           };
          
           FTValues values_;
          };
          
          } // namespace test_hardware_components
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  
           test_hardware_components::TestForceTorqueSensor,   hardware_interface::SensorInterface )

ros2_control/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/actuator_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::ActuatorInterface;
          using hardware_interface::CommandInterface;
          using hardware_interface::return_type;
          using hardware_interface::StateInterface;
          
          namespace test_hardware_components
          {
      28  class TestSingleJointActuator : public ActuatorInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & actuator_info ) override
           {
           if (  ActuatorInterface::on_init(  actuator_info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           // can only control one joint
           if (  info_.joints.size(   ) != 1 )
           {
           return CallbackReturn::ERROR;
           }
           // can only control in position
           const auto & command_interfaces = info_.joints[0].command_interfaces;
           if (  command_interfaces.size(   ) != 1 )
           {
           return CallbackReturn::ERROR;
           }
           if (  command_interfaces[0].name != hardware_interface::HW_IF_POSITION )
           {
           return CallbackReturn::ERROR;
           }
           // can only give feedback state for position and velocity
           const auto & state_interfaces = info_.joints[0].state_interfaces;
           if (  state_interfaces.size(   ) < 1 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const auto & state_interface : state_interfaces )
           {
           if (  
           (  state_interface.name != hardware_interface::HW_IF_POSITION ) &&
           (  state_interface.name != hardware_interface::HW_IF_VELOCITY ) )
           {
           return CallbackReturn::ERROR;
           }
           }
           fprintf(  stderr,   "TestSingleJointActuator configured successfully.\n" );
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
          
           const auto & joint_name = info_.joints[0].name;
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           joint_name,   hardware_interface::HW_IF_POSITION,   &position_state_ ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           joint_name,   hardware_interface::HW_IF_VELOCITY,   &velocity_state_ ) );
          
           return state_interfaces;
           }
          
           std::vector<CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<CommandInterface> command_interfaces;
          
           const auto & joint_name = info_.joints[0].name;
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           joint_name,   hardware_interface::HW_IF_POSITION,   &position_command_ ) );
          
           return command_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
           return_type write(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           velocity_state_ = position_command_ - position_state_;
           position_state_ = position_command_;
           return return_type::OK;
           }
          
          private:
           double position_state_ = 0.0;
           double velocity_state_ = 0.0;
           double position_command_ = 0.0;
          };
          
          } // namespace test_hardware_components
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  
           test_hardware_components::TestSingleJointActuator,   hardware_interface::ActuatorInterface )

ros2_control/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp

          // Copyright 2021 Department of Engineering Cybernetics,   NTNU
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <array>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          namespace test_hardware_components
          {
      24  class TestSystemCommandModes : public hardware_interface::SystemInterface
          {
          public:
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & system_info ) override
           {
           if (  hardware_interface::SystemInterface::on_init(  system_info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           // Can only control two joints
           if (  info_.joints.size(   ) != 2 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const hardware_interface::ComponentInfo & joint : info_.joints )
           {
           // Can control in position or velocity
           const auto & command_interfaces = joint.command_interfaces;
           if (  command_interfaces.size(   ) != 2 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const auto & command_interface : command_interfaces )
           {
           if (  
           command_interface.name != hardware_interface::HW_IF_POSITION &&
           command_interface.name != hardware_interface::HW_IF_VELOCITY )
           {
           return CallbackReturn::ERROR;
           }
           }
           // Can give feedback state for position,   velocity,   and acceleration
           const auto & state_interfaces = joint.state_interfaces;
           if (  state_interfaces.size(   ) != 2 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const auto & state_interface : state_interfaces )
           {
           if (  
           state_interface.name != hardware_interface::HW_IF_POSITION &&
           state_interface.name != hardware_interface::HW_IF_VELOCITY )
           {
           return CallbackReturn::ERROR;
           }
           }
           }
          
           fprintf(  stderr,   "TestSystemCommandModes configured successfully.\n" );
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override
           {
           std::vector<hardware_interface::StateInterface> state_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_POSITION,   &position_state_[i] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_VELOCITY,   &velocity_state_[i] ) );
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_ACCELERATION,   &acceleration_state_[i] ) );
           }
          
           return state_interfaces;
           }
          
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<hardware_interface::CommandInterface> command_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_POSITION,   &position_command_[i] ) );
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_VELOCITY,   &velocity_command_[i] ) );
           }
          
           return command_interfaces;
           }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type write(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type prepare_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & stop_interfaces ) override
           {
           // Starting interfaces
           start_modes_.clear(   );
           stop_modes_.clear(   );
           for (  const auto & key : start_interfaces )
           {
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           if (  key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION )
           {
           start_modes_.push_back(  hardware_interface::HW_IF_POSITION );
           }
           if (  key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY )
           {
           start_modes_.push_back(  hardware_interface::HW_IF_VELOCITY );
           }
           }
           }
           // Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time
           if (  start_modes_.size(   ) != 0 && start_modes_.size(   ) != info_.joints.size(   ) )
           {
           return hardware_interface::return_type::ERROR;
           }
          
           // Stopping interfaces
           for (  const auto & key : stop_interfaces )
           {
           for (  auto i = 0u; i < info_.joints.size(   ); i++ )
           {
           if (  key.find(  info_.joints[i].name ) != std::string::npos )
           {
           stop_modes_.push_back(  true );
           }
           }
           }
           // Example Criteria 2 - Stopping: All joints must have the same command mode
           if (  stop_modes_.size(   ) != 0 && stop_modes_.size(   ) != 2 && stop_modes_[0] != stop_modes_[1] )
           {
           return hardware_interface::return_type::ERROR;
           }
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type perform_command_mode_switch(  
           const std::vector<std::string> & start_interfaces,  
           const std::vector<std::string> & /*stop_interfaces*/ ) override
           {
           // Test of failure in perform command mode switch
           // Fail if given an empty list.
           // This should never occur in a real system as the same start_interfaces list is sent to both
           // prepare and perform,   and an error should be handled in prepare.
           if (  start_interfaces.size(   ) == 0 )
           {
           return hardware_interface::return_type::ERROR;
           }
           return hardware_interface::return_type::OK;
           }
          
          private:
           std::vector<std::string> start_modes_ = {"position",   "position"};
           std::vector<bool> stop_modes_ = {false,   false};
          
           std::array<double,   2> position_command_ = {0.0,   0.0};
           std::array<double,   2> velocity_command_ = {0.0,   0.0};
           std::array<double,   2> position_state_ = {0.0,   0.0};
           std::array<double,   2> velocity_state_ = {0.0,   0.0};
           std::array<double,   2> acceleration_state_ = {0.0,   0.0};
          };
          
          } // namespace test_hardware_components
          
          #include "pluginlib/class_list_macros.hpp"
          PLUGINLIB_EXPORT_CLASS(  
           test_hardware_components::TestSystemCommandModes,   hardware_interface::SystemInterface )

ros2_control/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <array>
          #include <memory>
          #include <vector>
          
          #include "hardware_interface/system_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::return_type;
          using hardware_interface::StateInterface;
          using hardware_interface::SystemInterface;
          
          namespace test_hardware_components
          {
      29  class TestTwoJointSystem : public SystemInterface
          {
           CallbackReturn on_init(  const hardware_interface::HardwareInfo & system_info ) override
           {
           if (  SystemInterface::on_init(  system_info ) != CallbackReturn::SUCCESS )
           {
           return CallbackReturn::ERROR;
           }
          
           // can only control two joint
           if (  info_.joints.size(   ) != 2 )
           {
           return CallbackReturn::ERROR;
           }
           for (  const auto & joint : info_.joints )
           {
           // can only control in position
           const auto & command_interfaces = joint.command_interfaces;
           if (  command_interfaces.size(   ) != 1 )
           {
           return CallbackReturn::ERROR;
           }
           if (  command_interfaces[0].name != hardware_interface::HW_IF_POSITION )
           {
           return CallbackReturn::ERROR;
           }
           // can only give feedback state for position and velocity
           const auto & state_interfaces = joint.state_interfaces;
           if (  state_interfaces.size(   ) != 1 )
           {
           return CallbackReturn::ERROR;
           }
           if (  state_interfaces[0].name != hardware_interface::HW_IF_POSITION )
           {
           return CallbackReturn::ERROR;
           }
           }
          
           fprintf(  stderr,   "TestTwoJointSystem configured successfully.\n" );
           return CallbackReturn::SUCCESS;
           }
          
           std::vector<StateInterface> export_state_interfaces(   ) override
           {
           std::vector<StateInterface> state_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); ++i )
           {
           state_interfaces.emplace_back(  hardware_interface::StateInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_POSITION,   &position_state_[i] ) );
           }
          
           return state_interfaces;
           }
          
           std::vector<CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<CommandInterface> command_interfaces;
           for (  auto i = 0u; i < info_.joints.size(   ); ++i )
           {
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           info_.joints[i].name,   hardware_interface::HW_IF_POSITION,   &position_command_[i] ) );
           }
          
           return command_interfaces;
           }
          
           return_type read(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
           return_type write(  const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return return_type::OK;
           }
          
          private:
           std::array<double,   2> position_command_ = {0.0,   0.0};
           std::array<double,   2> position_state_ = {0.0,   0.0};
          };
          
          } // namespace test_hardware_components
          
          #include "pluginlib/class_list_macros.hpp" // NOLINT
          PLUGINLIB_EXPORT_CLASS(  
           test_hardware_components::TestTwoJointSystem,   hardware_interface::SystemInterface )

ros2_control/hardware_interface/test/test_macros.cpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <vector>
          
          #include "hardware_interface/macros.hpp"
          
          #include "gmock/gmock.h"
          
      21  class TestMacros : public ::testing::Test
          {
          protected:
      24   static void SetUpTestCase(   ) {}
          };
          
      27  class A
          {
          };
          
      31  TEST_F(  TestMacros,   throw_on_null )
          {
           int * i_ptr = nullptr;
           // cppcheck-suppress unknownMacro
           EXPECT_ANY_THROW(  THROW_ON_NULLPTR(  i_ptr ) );
          
           A * a_ptr = nullptr;
           EXPECT_ANY_THROW(  THROW_ON_NULLPTR(  a_ptr ) );
          
           std::vector<int *> vec_ptr(  7 );
           for (  auto ptr : vec_ptr )
           {
           EXPECT_ANY_THROW(  THROW_ON_NULLPTR(  ptr ) );
           }
          
           int ** i_ptr_ptr = &i_ptr;
           EXPECT_ANY_THROW(  THROW_ON_NULLPTR(  *i_ptr_ptr ) );
          
           /*
           // undefined behavior
           int * i_ptr;
           THROW_ON_NULLPTR(  i_ptr );
           */
          }
          
      56  TEST_F(  TestMacros,   throw_on_not_null )
          {
           int * i_ptr = new int(   );
           EXPECT_ANY_THROW(  THROW_ON_NOT_NULLPTR(  i_ptr ) );
          
           A * a_ptr = new A(   );
           EXPECT_ANY_THROW(  THROW_ON_NOT_NULLPTR(  a_ptr ) );
          
           std::vector<int *> vec_ptr;
           for (  auto i : {0,   1,   2} )
           {
           vec_ptr.push_back(  i_ptr );
           EXPECT_ANY_THROW(  THROW_ON_NOT_NULLPTR(  vec_ptr[i] ) );
           }
          
           int ** i_ptr_ptr = &i_ptr;
           EXPECT_ANY_THROW(  THROW_ON_NOT_NULLPTR(  *i_ptr_ptr ) );
          
           delete a_ptr;
          }

ros2_control/hardware_interface/test/test_resource_manager.cpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          // Authors: Karsten Knese,   Denis Stogl
          
          #include <gmock/gmock.h>
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/actuator_interface.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "hardware_interface/types/lifecycle_state_names.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES;
          using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE;
          
      48  class TestResourceManager : public ::testing::Test
          {
          public:
      51   static void SetUpTestCase(   ) {}
          
      53   void SetUp(   ) {}
          };
          
      56  void set_components_state(  
           hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components,  
           const uint8_t state_id,   const std::string & state_name )
          {
           auto int_components = components;
           if (  int_components.empty(   ) )
           {
           int_components = {"TestActuatorHardware",   "TestSensorHardware",   "TestSystemHardware"};
           }
           for (  const auto & component : int_components )
           {
           rclcpp_lifecycle::State state(  state_id,   state_name );
           rm.set_component_state(  component,   state );
           }
          }
          
          auto configure_components =
           [](  hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components = {} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           hardware_interface::lifecycle_state_names::INACTIVE );
          };
          
          auto activate_components =
           [](  hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components = {} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,  
           hardware_interface::lifecycle_state_names::ACTIVE );
          };
          
          auto deactivate_components =
           [](  hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components = {} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,  
           hardware_interface::lifecycle_state_names::INACTIVE );
          };
          
          auto cleanup_components =
           [](  hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components = {} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
          };
          
          auto shutdown_components =
           [](  hardware_interface::ResourceManager & rm,   const std::vector<std::string> & components = {} )
          {
           set_components_state(  
           rm,   components,   lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,  
           hardware_interface::lifecycle_state_names::FINALIZED );
          };
          
          TEST_F(  TestResourceManager,   initialization_empty )
          {
           ASSERT_ANY_THROW(  hardware_interface::ResourceManager rm(  "" ) );
          }
          
          TEST_F(  TestResourceManager,   initialization_with_urdf )
          {
           ASSERT_NO_THROW(  
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf ) );
          }
          
          TEST_F(  TestResourceManager,   post_initialization_with_urdf )
          {
           hardware_interface::ResourceManager rm;
           ASSERT_NO_THROW(  rm.load_urdf(  ros2_control_test_assets::minimal_robot_urdf ) );
          }
          
          TEST_F(  TestResourceManager,   initialization_with_urdf_manual_validation )
          {
           // we validate the results manually
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf,   false );
          
           EXPECT_EQ(  1u,   rm.actuator_components_size(   ) );
           EXPECT_EQ(  1u,   rm.sensor_components_size(   ) );
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
          
           auto state_interface_keys = rm.state_interface_keys(   );
           ASSERT_EQ(  11u,   state_interface_keys.size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "sensor1/velocity" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint2/position" ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "joint3/position" ) );
          
           auto command_interface_keys = rm.command_interface_keys(   );
           ASSERT_EQ(  6u,   command_interface_keys.size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint1/position" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint2/velocity" ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "joint3/velocity" ) );
          }
          
          TEST_F(  TestResourceManager,   initialization_with_wrong_urdf )
          {
           // missing state keys
           {
           EXPECT_THROW(  
           hardware_interface::ResourceManager rm(  
           ros2_control_test_assets::minimal_robot_missing_state_keys_urdf ),  
           std::exception );
           }
           // missing command keys
           {
           EXPECT_THROW(  
           hardware_interface::ResourceManager rm(  
           ros2_control_test_assets::minimal_robot_missing_command_keys_urdf ),  
           std::exception );
           }
          }
          
          TEST_F(  TestResourceManager,   initialization_with_urdf_unclaimed )
          {
           // we validate the results manually
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           auto command_interface_keys = rm.command_interface_keys(   );
           for (  const auto & key : command_interface_keys )
           {
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
           }
           // state interfaces don't have to be locked,   hence any arbitrary key
           // should return false.
           auto state_interface_keys = rm.state_interface_keys(   );
           for (  const auto & key : state_interface_keys )
           {
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
           }
          }
          
          TEST_F(  TestResourceManager,   resource_claiming )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           const auto key = "joint1/position";
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
          
           {
           auto position_command_interface = rm.claim_command_interface(  key );
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_TRUE(  rm.command_interface_is_claimed(  key ) );
           {
           EXPECT_ANY_THROW(  rm.claim_command_interface(  key ) );
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           }
           }
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
          
           // command interfaces can only be claimed once
           for (  const auto & key :
           {"joint1/position",   "joint1/position",   "joint1/position",   "joint2/velocity",  
           "joint3/velocity"} )
           {
           {
           auto interface = rm.claim_command_interface(  key );
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_TRUE(  rm.command_interface_is_claimed(  key ) );
           {
           EXPECT_ANY_THROW(  rm.claim_command_interface(  key ) );
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           }
           }
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
           }
          
           // TODO(  destogl ): This claim test is not true.... can not be...
           // state interfaces can be claimed multiple times
           for (  const auto & key :
           {"joint1/position",   "joint1/velocity",   "sensor1/velocity",   "joint2/position",  
           "joint3/position"} )
           {
           {
           EXPECT_TRUE(  rm.state_interface_is_available(  key ) );
           auto interface = rm.claim_state_interface(  key );
           {
           EXPECT_TRUE(  rm.state_interface_is_available(  key ) );
           EXPECT_NO_THROW(  rm.claim_state_interface(  key ) );
           }
           }
           }
          
           std::vector<hardware_interface::LoanedCommandInterface> interfaces;
           const auto interface_names = {"joint1/position",   "joint2/velocity",   "joint3/velocity"};
           for (  const auto & key : interface_names )
           {
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           interfaces.emplace_back(  rm.claim_command_interface(  key ) );
           }
           for (  const auto & key : interface_names )
           {
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_TRUE(  rm.command_interface_is_claimed(  key ) );
           }
           interfaces.clear(   );
           for (  const auto & key : interface_names )
           {
           EXPECT_TRUE(  rm.command_interface_is_available(  key ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  key ) );
           }
          }
          
          class ExternalComponent : public hardware_interface::ActuatorInterface
          {
           std::vector<hardware_interface::StateInterface> export_state_interfaces(   ) override
           {
           std::vector<hardware_interface::StateInterface> state_interfaces;
           state_interfaces.emplace_back(  
           hardware_interface::StateInterface(  "external_joint",   "external_state_interface",   nullptr ) );
          
           return state_interfaces;
           }
          
           std::vector<hardware_interface::CommandInterface> export_command_interfaces(   ) override
           {
           std::vector<hardware_interface::CommandInterface> command_interfaces;
           command_interfaces.emplace_back(  hardware_interface::CommandInterface(  
           "external_joint",   "external_command_interface",   nullptr ) );
          
           return command_interfaces;
           }
          
           std::string get_name(   ) const override { return "ExternalComponent"; }
          
           hardware_interface::return_type read(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          
           hardware_interface::return_type write(  
           const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ ) override
           {
           return hardware_interface::return_type::OK;
           }
          };
          
          TEST_F(  TestResourceManager,   post_initialization_add_components )
          {
           // we validate the results manually
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf,   false );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           EXPECT_EQ(  1u,   rm.actuator_components_size(   ) );
           EXPECT_EQ(  1u,   rm.sensor_components_size(   ) );
           EXPECT_EQ(  1u,   rm.system_components_size(   ) );
          
           ASSERT_EQ(  11u,   rm.state_interface_keys(   ).size(   ) );
           ASSERT_EQ(  6u,   rm.command_interface_keys(   ).size(   ) );
          
           hardware_interface::HardwareInfo external_component_hw_info;
           external_component_hw_info.name = "ExternalComponent";
           external_component_hw_info.type = "actuator";
           rm.import_component(  std::make_unique<ExternalComponent>(   ),   external_component_hw_info );
           EXPECT_EQ(  2u,   rm.actuator_components_size(   ) );
          
           ASSERT_EQ(  12u,   rm.state_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.state_interface_exists(  "external_joint/external_state_interface" ) );
           ASSERT_EQ(  7u,   rm.command_interface_keys(   ).size(   ) );
           EXPECT_TRUE(  rm.command_interface_exists(  "external_joint/external_command_interface" ) );
          
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["ExternalComponent"].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
          
           configure_components(  rm,   {"ExternalComponent"} );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["ExternalComponent"].state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           activate_components(  rm,   {"ExternalComponent"} );
           status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map["ExternalComponent"].state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           EXPECT_NO_THROW(  rm.claim_state_interface(  "external_joint/external_state_interface" ) );
           EXPECT_NO_THROW(  rm.claim_command_interface(  "external_joint/external_command_interface" ) );
          }
          
          TEST_F(  TestResourceManager,   default_prepare_perform_switch )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
           // Activate components to get all interfaces available
           activate_components(  rm );
          
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  {""},   {""} ) );
           EXPECT_TRUE(  rm.perform_command_mode_switch(  {""},   {""} ) );
          }
          
          const auto hardware_resources_command_modes =
           R"(  
           <ros2_control name="TestSystemCommandModes" type="system">
           <hardware>
           <plugin>test_hardware_components/TestSystemCommandModes</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           )";
          const auto command_mode_urdf = std::string(  ros2_control_test_assets::urdf_head ) +
           std::string(  hardware_resources_command_modes ) +
           std::string(  ros2_control_test_assets::urdf_tail );
          
          TEST_F(  TestResourceManager,   custom_prepare_perform_switch )
          {
           hardware_interface::ResourceManager rm(  command_mode_urdf );
           // Scenarios defined by example criteria
           std::vector<std::string> empty_keys = {};
           std::vector<std::string> irrelevant_keys = {"elbow_joint/position",   "should_joint/position"};
           std::vector<std::string> illegal_single_key = {"joint1/position"};
           std::vector<std::string> legal_keys_position = {"joint1/position",   "joint2/position"};
           std::vector<std::string> legal_keys_velocity = {"joint1/velocity",   "joint2/velocity"};
           // Default behavior for empty key lists
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  empty_keys,   empty_keys ) );
          
           // Default behavior when given irrelevant keys
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  irrelevant_keys,   irrelevant_keys ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  irrelevant_keys,   empty_keys ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  empty_keys,   irrelevant_keys ) );
          
           // The test hardware interface has a criteria that both joints must change mode
           EXPECT_FALSE(  rm.prepare_command_mode_switch(  illegal_single_key,   illegal_single_key ) );
           EXPECT_FALSE(  rm.prepare_command_mode_switch(  illegal_single_key,   empty_keys ) );
           EXPECT_FALSE(  rm.prepare_command_mode_switch(  empty_keys,   illegal_single_key ) );
          
           // Test legal start keys
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  legal_keys_position,   legal_keys_position ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  legal_keys_velocity,   legal_keys_velocity ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  legal_keys_position,   empty_keys ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  empty_keys,   legal_keys_position ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  legal_keys_velocity,   empty_keys ) );
           EXPECT_TRUE(  rm.prepare_command_mode_switch(  empty_keys,   legal_keys_velocity ) );
          
           // Test rejection from perform_command_mode_switch,   test hardware rejects empty start sets
           EXPECT_TRUE(  rm.perform_command_mode_switch(  legal_keys_position,   legal_keys_position ) );
           EXPECT_FALSE(  rm.perform_command_mode_switch(  empty_keys,   empty_keys ) );
           EXPECT_FALSE(  rm.perform_command_mode_switch(  empty_keys,   legal_keys_position ) );
          }
          
          TEST_F(  TestResourceManager,   resource_status )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           auto status_map = rm.get_components_status(   );
          
           // name
           EXPECT_EQ(  status_map[TEST_ACTUATOR_HARDWARE_NAME].name,   TEST_ACTUATOR_HARDWARE_NAME );
           EXPECT_EQ(  status_map[TEST_SENSOR_HARDWARE_NAME].name,   TEST_SENSOR_HARDWARE_NAME );
           EXPECT_EQ(  status_map[TEST_SYSTEM_HARDWARE_NAME].name,   TEST_SYSTEM_HARDWARE_NAME );
           // type
           EXPECT_EQ(  status_map[TEST_ACTUATOR_HARDWARE_NAME].type,   TEST_ACTUATOR_HARDWARE_TYPE );
           EXPECT_EQ(  status_map[TEST_SENSOR_HARDWARE_NAME].type,   TEST_SENSOR_HARDWARE_TYPE );
           EXPECT_EQ(  status_map[TEST_SYSTEM_HARDWARE_NAME].type,   TEST_SYSTEM_HARDWARE_TYPE );
           // class_type
           EXPECT_EQ(  status_map[TEST_ACTUATOR_HARDWARE_NAME].class_type,   TEST_ACTUATOR_HARDWARE_CLASS_TYPE );
           EXPECT_EQ(  status_map[TEST_SENSOR_HARDWARE_NAME].class_type,   TEST_SENSOR_HARDWARE_CLASS_TYPE );
           EXPECT_EQ(  status_map[TEST_SYSTEM_HARDWARE_NAME].class_type,   TEST_SYSTEM_HARDWARE_CLASS_TYPE );
           // state
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
          
           auto check_interfaces = [](  
           const std::vector<std::string> & registered_interfaces,  
           const std::vector<std::string> & interface_names )
           {
           for (  const std::string & interface : interface_names )
           {
           auto it = std::find(  registered_interfaces.begin(   ),   registered_interfaces.end(   ),   interface );
           EXPECT_NE(  it,   registered_interfaces.end(   ) );
           }
           };
          
           check_interfaces(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].command_interfaces,  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES );
           EXPECT_TRUE(  status_map[TEST_SENSOR_HARDWARE_NAME].command_interfaces.empty(   ) );
           check_interfaces(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].command_interfaces,  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES );
          
           check_interfaces(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces,  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES );
           EXPECT_NE(  
           std::find(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.begin(   ),  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.end(   ),  
           "joint1/some_unlisted_interface" ),  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.end(   ) );
           check_interfaces(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state_interfaces,   TEST_SENSOR_HARDWARE_STATE_INTERFACES );
           check_interfaces(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces,   TEST_SYSTEM_HARDWARE_STATE_INTERFACES );
          }
          
          TEST_F(  TestResourceManager,   lifecycle_all_resources )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           // All resources start as UNCONFIGURED
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           }
          
           configure_components(  rm );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           }
          
           activate_components(  rm );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           }
          
           deactivate_components(  rm );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           }
          
           cleanup_components(  rm );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           }
          
           shutdown_components(  rm );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           }
          }
          
          TEST_F(  TestResourceManager,   lifecycle_individual_resources )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           // All resources start as UNCONFIGURED
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           }
          
           configure_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           }
          
           activate_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           }
          
           configure_components(  rm,   {TEST_SENSOR_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           }
          
           activate_components(  rm,   {TEST_SENSOR_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           }
          
           deactivate_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME,   TEST_SENSOR_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           }
          
           cleanup_components(  rm,   {TEST_SENSOR_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::INACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::ACTIVE );
           }
          
           shutdown_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::UNCONFIGURED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           }
          
           // TODO(  destogl ): Watch-out this fails in output,   why is this not caught?!!!
           shutdown_components(  rm,   {TEST_SENSOR_HARDWARE_NAME} );
           {
           auto status_map = rm.get_components_status(   );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SENSOR_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(   ),  
           lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
           EXPECT_EQ(  
           status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(   ),  
           hardware_interface::lifecycle_state_names::FINALIZED );
           }
          }
          
          TEST_F(  TestResourceManager,   resource_availability_and_claiming_in_lifecycle )
          {
           using std::placeholders::_1;
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           auto check_interfaces =
           [](  const std::vector<std::string> & interface_names,   auto check_method,   bool expected_result )
           {
           for (  const auto & interface : interface_names )
           {
           EXPECT_EQ(  check_method(  interface ),   expected_result );
           }
           };
          
           auto check_interface_claiming = [&](  
           const std::vector<std::string> & state_interface_names,  
           const std::vector<std::string> & command_interface_names,  
           bool expected_result )
           {
           std::vector<hardware_interface::LoanedStateInterface> states;
           std::vector<hardware_interface::LoanedCommandInterface> commands;
          
           if (  expected_result )
           {
           for (  const auto & key : state_interface_names )
           {
           EXPECT_NO_THROW(  states.emplace_back(  rm.claim_state_interface(  key ) ) );
           }
           for (  const auto & key : command_interface_names )
           {
           EXPECT_NO_THROW(  commands.emplace_back(  rm.claim_command_interface(  key ) ) );
           }
           }
           else
           {
           for (  const auto & key : state_interface_names )
           {
           EXPECT_ANY_THROW(  states.emplace_back(  rm.claim_state_interface(  key ) ) );
           }
           for (  const auto & key : command_interface_names )
           {
           EXPECT_ANY_THROW(  commands.emplace_back(  rm.claim_command_interface(  key ) ) );
           }
           }
          
           check_interfaces(  
           command_interface_names,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_claimed,   &rm,   _1 ),  
           expected_result );
           };
          
           // All resources start as UNCONFIGURED - All interfaces are imported but not available
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           false );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           }
          
           // Nothing can be claimed
           {
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,   false );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   false );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   false );
           }
          
           // When actuator is configured all interfaces become available
           configure_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME} );
           {
           check_interfaces(  
           {"joint1/position"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           {"joint1/max_velocity"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           false );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           }
          
           // Can claim Actuator's interfaces
           {
           check_interface_claiming(  {},   {"joint1/position"},   true );
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   {"joint1/max_velocity"},   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   false );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   false );
           }
          
           // When actuator is activated all state- and command- interfaces become available
           activate_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME} );
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           false );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           }
          
           // Can claim all Actuator's state interfaces and command interfaces
           {
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   false );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   false );
           }
          
           // Check if all interfaces still exits
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           }
          
           // When Sensor and System are configured their state-
           // and command- interfaces are available
           configure_components(  rm,   {TEST_SENSOR_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_NAME} );
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           {"joint2/velocity",   "joint3/velocity"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           {"joint2/max_acceleration",   "configuration/max_tcp_jerk"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           }
          
           // Can claim:
           // - all Actuator's state interfaces and command interfaces
           // - sensor's state interfaces
           // - system's state and command interfaces
           {
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   true );
           check_interface_claiming(  {},   {"joint2/velocity",   "joint3/velocity"},   true );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           {"joint2/max_acceleration",   "configuration/max_tcp_jerk"},   true );
           }
          
           // All active - everything available
           activate_components(  rm,   {TEST_SENSOR_HARDWARE_NAME,   TEST_SYSTEM_HARDWARE_NAME} );
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           }
          
           // Can claim everything
           // - actuator's state interfaces and command interfaces
           // - sensor's state interfaces
           // - system's state and non-moving command interfaces
           {
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   true );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   true );
           }
          
           // When deactivated - movement interfaces are not available anymore
           deactivate_components(  rm,   {TEST_ACTUATOR_HARDWARE_NAME,   TEST_SENSOR_HARDWARE_NAME} );
           {
           check_interfaces(  
           {"joint1/position"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           {"joint1/max_velocity"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           }
          
           // Can claim everything
           // - actuator's state and command interfaces
           // - sensor's state interfaces
           // - system's state and command interfaces
           {
           check_interface_claiming(  {},   {"joint1/position"},   true );
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   {"joint1/max_velocity"},   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   true );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   true );
           }
          
           // When sensor is cleaned up the interfaces are not available anymore
           cleanup_components(  rm,   {TEST_SENSOR_HARDWARE_NAME} );
           {
           check_interfaces(  
           {"joint1/position"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           {"joint1/max_velocity"},  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_is_available,   &rm,   _1 ),  
           true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),  
           false );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_is_available,   &rm,   _1 ),   true );
           }
          
           // Can claim everything
           // - actuator's state and command interfaces
           // - no sensor's interface
           // - system's state and command interfaces
           {
           check_interface_claiming(  {},   {"joint1/position"},   true );
           check_interface_claiming(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,   {"joint1/max_velocity"},   true );
           check_interface_claiming(  TEST_SENSOR_HARDWARE_STATE_INTERFACES,   {},   false );
           check_interface_claiming(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,   TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,   true );
           }
          
           // Check if all interfaces still exits
           {
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::command_interface_exists,   &rm,   _1 ),   true );
          
           check_interfaces(  
           TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SENSOR_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           check_interfaces(  
           TEST_SYSTEM_HARDWARE_STATE_INTERFACES,  
           std::bind(  &hardware_interface::ResourceManager::state_interface_exists,   &rm,   _1 ),   true );
           }
          }
          
          TEST_F(  TestResourceManager,   managing_controllers_reference_interfaces )
          {
           hardware_interface::ResourceManager rm(  ros2_control_test_assets::minimal_robot_urdf );
          
           std::string CONTROLLER_NAME = "test_controller";
           std::vector<std::string> REFERENCE_INTERFACE_NAMES = {"input1",   "input2",   "input3"};
           std::vector<std::string> FULL_REFERENCE_INTERFACE_NAMES = {
           CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[0],  
           CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1],  
           CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]};
          
           std::vector<hardware_interface::CommandInterface> reference_interfaces;
           std::vector<double> reference_interface_values = {1.0,   2.0,   3.0};
          
           for (  size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(   ); ++i )
           {
           reference_interfaces.push_back(  hardware_interface::CommandInterface(  
           CONTROLLER_NAME,   REFERENCE_INTERFACE_NAMES[i],   &(  reference_interface_values[i] ) ) );
           }
          
           rm.import_controller_reference_interfaces(  CONTROLLER_NAME,   reference_interfaces );
          
           ASSERT_THAT(  
           rm.get_controller_reference_interface_names(  CONTROLLER_NAME ),  
           testing::ElementsAreArray(  FULL_REFERENCE_INTERFACE_NAMES ) );
          
           // check that all interfaces are imported properly
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_TRUE(  rm.command_interface_exists(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_available(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  interface ) );
           }
          
           // make interface available
           rm.make_controller_reference_interfaces_available(  CONTROLLER_NAME );
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_TRUE(  rm.command_interface_exists(  interface ) );
           EXPECT_TRUE(  rm.command_interface_is_available(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  interface ) );
           }
          
           // try to make interfaces available from unknown controller
           EXPECT_THROW(  
           rm.make_controller_reference_interfaces_available(  "unknown_controller" ),   std::out_of_range );
          
           // claim interfaces in a scope that deletes them after
           {
           auto claimed_itf1 = rm.claim_command_interface(  FULL_REFERENCE_INTERFACE_NAMES[0] );
           auto claimed_itf3 = rm.claim_command_interface(  FULL_REFERENCE_INTERFACE_NAMES[2] );
          
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_TRUE(  rm.command_interface_exists(  interface ) );
           EXPECT_TRUE(  rm.command_interface_is_available(  interface ) );
           }
           EXPECT_TRUE(  rm.command_interface_is_claimed(  FULL_REFERENCE_INTERFACE_NAMES[0] ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  FULL_REFERENCE_INTERFACE_NAMES[1] ) );
           EXPECT_TRUE(  rm.command_interface_is_claimed(  FULL_REFERENCE_INTERFACE_NAMES[2] ) );
          
           // access interface value
           EXPECT_EQ(  claimed_itf1.get_value(   ),   1.0 );
           EXPECT_EQ(  claimed_itf3.get_value(   ),   3.0 );
          
           claimed_itf1.set_value(  11.1 );
           claimed_itf3.set_value(  33.3 );
           EXPECT_EQ(  claimed_itf1.get_value(   ),   11.1 );
           EXPECT_EQ(  claimed_itf3.get_value(   ),   33.3 );
          
           EXPECT_EQ(  reference_interface_values[0],   11.1 );
           EXPECT_EQ(  reference_interface_values[1],   2.0 );
           EXPECT_EQ(  reference_interface_values[2],   33.3 );
           }
          
           // interfaces should be released now,   but still managed by resource manager
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_TRUE(  rm.command_interface_exists(  interface ) );
           EXPECT_TRUE(  rm.command_interface_is_available(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  interface ) );
           }
          
           // make interfaces unavailable
           rm.make_controller_reference_interfaces_unavailable(  CONTROLLER_NAME );
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_TRUE(  rm.command_interface_exists(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_available(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_claimed(  interface ) );
           }
          
           // try to make interfaces unavailable from unknown controller
           EXPECT_THROW(  
           rm.make_controller_reference_interfaces_unavailable(  "unknown_controller" ),   std::out_of_range );
          
           // Last written values should stay
           EXPECT_EQ(  reference_interface_values[0],   11.1 );
           EXPECT_EQ(  reference_interface_values[1],   2.0 );
           EXPECT_EQ(  reference_interface_values[2],   33.3 );
          
           // remove reference interfaces from resource manager
           rm.remove_controller_reference_interfaces(  CONTROLLER_NAME );
          
           // they should not exist in resource manager
           for (  const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
           {
           EXPECT_FALSE(  rm.command_interface_exists(  interface ) );
           EXPECT_FALSE(  rm.command_interface_is_available(  interface ) );
           }
          
           // try to remove interfaces from unknown controller
           EXPECT_THROW(  
           rm.make_controller_reference_interfaces_unavailable(  "unknown_controller" ),   std::out_of_range );
          }

ros2_control/joint_limits/include/joint_limits/joint_limits.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_
          #define JOINT_LIMITS__JOINT_LIMITS_HPP_
          
          #include <limits>
          #include <sstream>
          #include <string>
          
          namespace joint_limits
          {
          /**
           * JointLimits structure stores values from from yaml definition or `<limits>` tag in URDF.
           * The mapping from URDF attributes to members is the following:
           * lower --> min_position
           * upper --> max_position
           * velocity --> max_velocity
           * effort --> max_effort
           */
          struct JointLimits
          {
           JointLimits(   )
           : min_position(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_position(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_velocity(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_acceleration(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_jerk(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_effort(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           has_position_limits(  false ),  
           has_velocity_limits(  false ),  
           has_acceleration_limits(  false ),  
           has_jerk_limits(  false ),  
           has_effort_limits(  false ),  
           angle_wraparound(  false )
           {
           }
          
           double min_position;
           double max_position;
           double max_velocity;
           double max_acceleration;
           double max_jerk;
           double max_effort;
          
           bool has_position_limits;
           bool has_velocity_limits;
           bool has_acceleration_limits;
           bool has_jerk_limits;
           bool has_effort_limits;
           bool angle_wraparound;
          
      66   std::string to_string(   )
           {
           std::stringstream ss_output;
          
           ss_output << " has position limits: " << (  has_position_limits ? "true" : "false" ) << "["
           << min_position << ",   " << max_position << "]\n";
           ss_output << " has velocity limits: " << (  has_velocity_limits ? "true" : "false" ) << "["
           << max_velocity << "]\n";
           ss_output << " has acceleration limits: " << (  has_acceleration_limits ? "true" : "false" )
           << " [" << max_acceleration << "]\n";
           ss_output << " has jerk limits: " << (  has_jerk_limits ? "true" : "false" ) << "[" << max_jerk
           << "]\n";
           ss_output << " has effort limits: " << (  has_effort_limits ? "true" : "false" ) << "["
           << max_effort << "]\n";
           ss_output << " angle wraparound: " << (  angle_wraparound ? "true" : "false" );
          
           return ss_output.str(   );
           }
          };
          
          /**
           * SoftJointLimits stores values from the `<safety_controller>` tag of URDF.
           * The meaning of the fields are:
           *
           * An element can contain the following attributes:
           *
           * **soft_lower_limit** (  optional,   defaults to 0 ) - An attribute specifying the lower joint boundary
           * where the safety controller starts limiting the position of the joint. This limit needs to be
           * larger than the lower joint limit (  see above ). See See safety limits for more details.
           *
           * **soft_upper_limit** (  optional,   defaults to 0 ) - An attribute specifying the upper joint boundary
           * where the safety controller starts limiting the position of the joint. This limit needs to be
           * smaller than the upper joint limit (  see above ). See See safety limits for more details.
           *
           * **k_position** (  optional,   defaults to 0 ) - An attribute specifying the relation between position
           * and velocity limits. See See safety limits for more details.
           *
           * k_velocity (  required ) - An attribute specifying the relation between effort and velocity limits.
           * See See safety limits for more details.
           */
          struct SoftJointLimits
          {
           SoftJointLimits(   )
           : min_position(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           max_position(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           k_position(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           k_velocity(  std::numeric_limits<double>::quiet_NaN(   ) )
           {
           }
          
           double min_position;
           double max_position;
           double k_position;
           double k_velocity;
          
     121   std::string to_string(   )
           {
           std::stringstream ss_output;
          
           ss_output << " soft position limits: "
           << "[" << min_position << ",   " << max_position << "]\n";
          
           ss_output << " k-position: "
           << "[" << k_position << "]\n";
          
           ss_output << " k-velocity: "
           << "[" << k_velocity << "]\n";
          
           return ss_output.str(   );
           }
          };
          
          } // namespace joint_limits
          
          #endif // JOINT_LIMITS__JOINT_LIMITS_HPP_

ros2_control/joint_limits/include/joint_limits/joint_limits_rosparam.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
          #define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
          
          #include <limits>
          #include <string>
          
          #include "joint_limits/joint_limits.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          
          namespace // utilities
          {
          /// Declare and initialize a parameter with a type.
          /**
           *
           * Wrapper function for templated node's declare_parameter(   ) which checks if
           * parameter is already declared.
           * For use in all components that inherit from ControllerInterface
           */
          template <typename ParameterT>
      37  auto auto_declare(  
           const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,  
           const std::string & name,   const ParameterT & default_value )
          {
           if (  !param_itf->has_parameter(  name ) )
           {
           auto param_default_value = rclcpp::ParameterValue(  default_value );
           param_itf->declare_parameter(  name,   param_default_value );
           }
           return param_itf->get_parameter(  name ).get_value<ParameterT>(   );
          }
          } // namespace
          
          namespace joint_limits
          {
          /**
           * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name using node
           * parameters interface \p param_itf.
           *
           * The following parameter structure is declared with base name `joint_limits.joint_name`:
           * \code
           * has_position_limits: bool
           * min_position: double
           * max_position: double
           * has_velocity_limits: bool
           * max_velocity: double
           * has_acceleration_limits: bool
           * max_acceleration: double
           * has_jerk_limits: bool
           * max_jerk: double
           * has_effort_limits: bool
           * max_effort: double
           * angle_wraparound: bool
           * has_soft_limits: bool
           * k_position: double
           * k_velocity: double
           * soft_lower_limit: double
           * soft_upper_limit: double
           * \endcode
           *
           * \param[in] joint_name name of the joint for which parameters will be declared.
           * \param[in] param_itf node parameters interface object to access parameters.
           * \param[in] logging_itf node logging interface to log if error happens.
           *
           * \returns True if parameters are successfully declared,   false otherwise.
           */
      83  inline bool declare_parameters(  
           const std::string & joint_name,  
           const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,  
           const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf )
          {
           const std::string param_base_name = "joint_limits." + joint_name;
           try
           {
           auto_declare<bool>(  param_itf,   param_base_name + ".has_position_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".min_position",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<double>(  
           param_itf,   param_base_name + ".max_position",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<bool>(  param_itf,   param_base_name + ".has_velocity_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".min_velocity",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<double>(  
           param_itf,   param_base_name + ".max_velocity",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<bool>(  param_itf,   param_base_name + ".has_acceleration_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".max_acceleration",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<bool>(  param_itf,   param_base_name + ".has_jerk_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".max_jerk",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<bool>(  param_itf,   param_base_name + ".has_effort_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".max_effort",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<bool>(  param_itf,   param_base_name + ".angle_wraparound",   false );
           auto_declare<bool>(  param_itf,   param_base_name + ".has_soft_limits",   false );
           auto_declare<double>(  
           param_itf,   param_base_name + ".k_position",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<double>(  
           param_itf,   param_base_name + ".k_velocity",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<double>(  
           param_itf,   param_base_name + ".soft_lower_limit",   std::numeric_limits<double>::quiet_NaN(   ) );
           auto_declare<double>(  
           param_itf,   param_base_name + ".soft_upper_limit",   std::numeric_limits<double>::quiet_NaN(   ) );
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  logging_itf->get_logger(   ),   "%s",   ex.what(   ) );
           return false;
           }
           return true;
          }
          
          /**
           * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name for the \p node
           * object.
           * This is a convenience function.
           * For parameters structure see the underlying `declare_parameters` function.
           *
           * \param[in] joint_name name of the joint for which parameters will be declared.
           * \param[in] node node for parameters should be declared.
           *
           * \returns True if parameters are successfully declared,   false otherwise.
           */
     140  inline bool declare_parameters(  const std::string & joint_name,   const rclcpp::Node::SharedPtr & node )
          {
           return declare_parameters(  
           joint_name,   node->get_node_parameters_interface(   ),   node->get_node_logging_interface(   ) );
          }
          
          /**
           * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node
           * object.
           * This is a convenience function.
           * For parameters structure see the underlying `declare_parameters` function.
           *
           * \param[in] joint_name name of the joint for which parameters will be declared.
           * \param[in] lifecycle_node lifecycle node for parameters should be declared.
           *
           * \returns True if parameters are successfully declared,   false otherwise.
           */
     157  inline bool declare_parameters(  
           const std::string & joint_name,   const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node )
          {
           return declare_parameters(  
           joint_name,   lifecycle_node->get_node_parameters_interface(   ),  
           lifecycle_node->get_node_logging_interface(   ) );
          }
          
          /// Populate a JointLimits instance from the node parameters.
          /**
           * It is assumed that parameter structure is the following:
           * \code
           * has_position_limits: bool
           * min_position: double
           * max_position: double
           * has_velocity_limits: bool
           * max_velocity: double
           * has_acceleration_limits: bool
           * max_acceleration: double
           * has_jerk_limits: bool
           * max_jerk: double
           * has_effort_limits: bool
           * max_effort: double
           * angle_wraparound: bool # will be ignored if there are position limits
           * \endcode
           *
           * Unspecified parameters are not added to the joint limits specification.
           * A specification in a yaml would look like this:
           * \code
           * <node_name>
           * ros__parameters:
           * joint_limits:
           * foo_joint:
           * has_position_limits: true
           * min_position: 0.0
           * max_position: 1.0
           * has_velocity_limits: true
           * max_velocity: 2.0
           * has_acceleration_limits: true
           * max_acceleration: 5.0
           * has_jerk_limits: true
           * max_jerk: 100.0
           * has_effort_limits: true
           * max_effort: 20.0
           * bar_joint:
           * has_position_limits: false # Continuous joint
           * angle_wraparound: true # available only for continuous joints
           * has_velocity_limits: true
           * max_velocity: 4.0
           * \endcode
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched,   e.g.,   "foo_joint".
           * \param[in] param_itf node parameters interface of the node where parameters are specified.
           * \param[in] logging_itf node logging interface to provide log errors.
           * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values. Values in \p limits not specified in the parameter server remain unchanged.
           *
           * \returns True if a limits specification is found (  i.e.,   the \p joint_limits/joint_name parameter exists in \p node ),   false otherwise.
           */
     216  inline bool get_joint_limits(  
           const std::string & joint_name,  
           const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,  
           const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,  
           JointLimits & limits )
          {
           const std::string param_base_name = "joint_limits." + joint_name;
           try
           {
           if (  
           !param_itf->has_parameter(  param_base_name + ".has_position_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".min_position" ) &&
           !param_itf->has_parameter(  param_base_name + ".max_position" ) &&
           !param_itf->has_parameter(  param_base_name + ".has_velocity_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".min_velocity" ) &&
           !param_itf->has_parameter(  param_base_name + ".max_velocity" ) &&
           !param_itf->has_parameter(  param_base_name + ".has_acceleration_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".max_acceleration" ) &&
           !param_itf->has_parameter(  param_base_name + ".has_jerk_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".max_jerk" ) &&
           !param_itf->has_parameter(  param_base_name + ".has_effort_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".max_effort" ) &&
           !param_itf->has_parameter(  param_base_name + ".angle_wraparound" ) &&
           !param_itf->has_parameter(  param_base_name + ".has_soft_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".k_position" ) &&
           !param_itf->has_parameter(  param_base_name + ".k_velocity" ) &&
           !param_itf->has_parameter(  param_base_name + ".soft_lower_limit" ) &&
           !param_itf->has_parameter(  param_base_name + ".soft_upper_limit" ) )
           {
           RCLCPP_ERROR(  
           logging_itf->get_logger(   ),  
           "No joint limits specification found for joint '%s' in the parameter server "
           "(  param name: %s ).",  
           joint_name.c_str(   ),   param_base_name.c_str(   ) );
           return false;
           }
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  logging_itf->get_logger(   ),   "%s",   ex.what(   ) );
           return false;
           }
          
           // Position limits
           if (  param_itf->has_parameter(  param_base_name + ".has_position_limits" ) )
           {
           limits.has_position_limits =
           param_itf->get_parameter(  param_base_name + ".has_position_limits" ).as_bool(   );
           if (  
           limits.has_position_limits && param_itf->has_parameter(  param_base_name + ".min_position" ) &&
           param_itf->has_parameter(  param_base_name + ".max_position" ) )
           {
           limits.min_position = param_itf->get_parameter(  param_base_name + ".min_position" ).as_double(   );
           limits.max_position = param_itf->get_parameter(  param_base_name + ".max_position" ).as_double(   );
           }
           else
           {
           limits.has_position_limits = false;
           }
          
           if (  
           !limits.has_position_limits &&
           param_itf->has_parameter(  param_base_name + ".angle_wraparound" ) )
           {
           limits.angle_wraparound =
           param_itf->get_parameter(  param_base_name + ".angle_wraparound" ).as_bool(   );
           }
           }
          
           // Velocity limits
           if (  param_itf->has_parameter(  param_base_name + ".has_velocity_limits" ) )
           {
           limits.has_velocity_limits =
           param_itf->get_parameter(  param_base_name + ".has_velocity_limits" ).as_bool(   );
           if (  limits.has_velocity_limits && param_itf->has_parameter(  param_base_name + ".max_velocity" ) )
           {
           limits.max_velocity = param_itf->get_parameter(  param_base_name + ".max_velocity" ).as_double(   );
           }
           else
           {
           limits.has_velocity_limits = false;
           }
           }
          
           // Acceleration limits
           if (  param_itf->has_parameter(  param_base_name + ".has_acceleration_limits" ) )
           {
           limits.has_acceleration_limits =
           param_itf->get_parameter(  param_base_name + ".has_acceleration_limits" ).as_bool(   );
           if (  
           limits.has_acceleration_limits &&
           param_itf->has_parameter(  param_base_name + ".max_acceleration" ) )
           {
           limits.max_acceleration =
           param_itf->get_parameter(  param_base_name + ".max_acceleration" ).as_double(   );
           }
           else
           {
           limits.has_acceleration_limits = false;
           }
           }
          
           // Jerk limits
           if (  param_itf->has_parameter(  param_base_name + ".has_jerk_limits" ) )
           {
           limits.has_jerk_limits =
           param_itf->get_parameter(  param_base_name + ".has_jerk_limits" ).as_bool(   );
           if (  limits.has_jerk_limits && param_itf->has_parameter(  param_base_name + ".max_jerk" ) )
           {
           limits.max_jerk = param_itf->get_parameter(  param_base_name + ".max_jerk" ).as_double(   );
           }
           else
           {
           limits.has_jerk_limits = false;
           }
           }
          
           // Effort limits
           if (  param_itf->has_parameter(  param_base_name + ".has_effort_limits" ) )
           {
           limits.has_effort_limits =
           param_itf->get_parameter(  param_base_name + ".has_effort_limits" ).as_bool(   );
           if (  limits.has_effort_limits && param_itf->has_parameter(  param_base_name + ".max_effort" ) )
           {
           limits.has_effort_limits = true;
           limits.max_effort = param_itf->get_parameter(  param_base_name + ".max_effort" ).as_double(   );
           }
           else
           {
           limits.has_effort_limits = false;
           }
           }
          
           return true;
          }
          
          /**
           * Populate a JointLimits instance from the node parameters.
           * This is a convenience function.
           * For parameters structure see the underlying `get_joint_limits` function.
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched.
           * \param[in] node Node object for which parameters should be fetched.
           * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values. Values in \p limits not specified in the parameter server remain unchanged.
           *
           * \returns True if a limits specification is found,   false otherwise.
           */
     364  inline bool get_joint_limits(  
           const std::string & joint_name,   const rclcpp::Node::SharedPtr & node,   JointLimits & limits )
          {
           return get_joint_limits(  
           joint_name,   node->get_node_parameters_interface(   ),   node->get_node_logging_interface(   ),   limits );
          }
          
          /**
           * Populate a JointLimits instance from the node parameters.
           * This is a convenience function.
           * For parameters structure see the underlying `get_joint_limits` function.
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched.
           * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched.
           * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values. Values in \p limits not specified in the parameter server remain unchanged.
           *
           * \returns True if a limits specification is found,   false otherwise.
           */
     383  inline bool get_joint_limits(  
           const std::string & joint_name,   const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,  
           JointLimits & limits )
          {
           return get_joint_limits(  
           joint_name,   lifecycle_node->get_node_parameters_interface(   ),  
           lifecycle_node->get_node_logging_interface(   ),   limits );
          }
          
          /// Populate a SoftJointLimits instance from the ROS parameter server.
          /**
           * It is assumed that the parameter structure is the following:
           * \code
           * has_soft_limits: bool
           * k_position: double
           * k_velocity: double
           * soft_lower_limit: double
           * soft_upper_limit: double
           * \endcode
           *
           * Only completely specified soft joint limits specifications will be considered valid.
           * For example a valid yaml configuration would look like:
           * \code
           * <node_name>
           * ros__parameters:
           * joint_limits:
           * foo_joint:
           * soft_lower_limit: 0.0
           * soft_upper_limit: 1.0
           * k_position: 10.0
           * k_velocity: 10.0
           * \endcode
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched,   e.g.,   "foo_joint".
           * \param[in] param_itf node parameters interface of the node where parameters are specified.
           * \param[in] logging_itf node logging interface to provide log errors.
           * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values.
           * \return True if a complete soft limits specification is found (  ie. if all \p k_position,   \p k_velocity,   \p soft_lower_limit and
           * \p soft_upper_limit exist in \p joint_limits/joint_name namespace ),   false otherwise.
           */
     424  inline bool get_joint_limits(  
           const std::string & joint_name,  
           const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,  
           const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,  
           SoftJointLimits & soft_limits )
          {
           const std::string param_base_name = "joint_limits." + joint_name;
           try
           {
           if (  
           !param_itf->has_parameter(  param_base_name + ".has_soft_limits" ) &&
           !param_itf->has_parameter(  param_base_name + ".k_velocity" ) &&
           !param_itf->has_parameter(  param_base_name + ".k_position" ) &&
           !param_itf->has_parameter(  param_base_name + ".soft_lower_limit" ) &&
           !param_itf->has_parameter(  param_base_name + ".soft_upper_limit" ) )
           {
           RCLCPP_DEBUG(  
           logging_itf->get_logger(   ),  
           "No soft joint limits specification found for joint '%s' in the parameter server "
           "(  param name: %s ).",  
           joint_name.c_str(   ),   param_base_name.c_str(   ) );
           return false;
           }
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  logging_itf->get_logger(   ),   "%s",   ex.what(   ) );
           return false;
           }
          
           // Override soft limits if complete specification is found
           if (  param_itf->has_parameter(  param_base_name + ".has_soft_limits" ) )
           {
           if (  
           param_itf->get_parameter(  param_base_name + ".has_soft_limits" ).as_bool(   ) &&
           param_itf->has_parameter(  param_base_name + ".k_position" ) &&
           param_itf->has_parameter(  param_base_name + ".k_velocity" ) &&
           param_itf->has_parameter(  param_base_name + ".soft_lower_limit" ) &&
           param_itf->has_parameter(  param_base_name + ".soft_upper_limit" ) )
           {
           soft_limits.k_position =
           param_itf->get_parameter(  param_base_name + ".k_position" ).as_double(   );
           soft_limits.k_velocity =
           param_itf->get_parameter(  param_base_name + ".k_velocity" ).as_double(   );
           soft_limits.min_position =
           param_itf->get_parameter(  param_base_name + ".soft_lower_limit" ).as_double(   );
           soft_limits.max_position =
           param_itf->get_parameter(  param_base_name + ".soft_upper_limit" ).as_double(   );
           return true;
           }
           }
          
           return false;
          }
          
          /**
           * Populate a JointLimits instance from the node parameters.
           * This is a convenience function.
           * For parameters structure see the underlying `get_joint_limits` function.
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched.
           * \param[in] node Node object for which parameters should be fetched.
           * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values.
           *
           * \returns True if a soft limits specification is found,   false otherwise.
           */
     491  inline bool get_joint_limits(  
           const std::string & joint_name,   const rclcpp::Node::SharedPtr & node,  
           SoftJointLimits & soft_limits )
          {
           return get_joint_limits(  
           joint_name,   node->get_node_parameters_interface(   ),   node->get_node_logging_interface(   ),  
           soft_limits );
          }
          
          /**
           * Populate a JointLimits instance from the node parameters.
           * This is a convenience function.
           * For parameters structure see the underlying `get_joint_limits` function.
           *
           * \param[in] joint_name Name of joint whose limits are to be fetched.
           * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched.
           * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
           * existing values.
           *
           * \returns True if a soft limits specification is found,   false otherwise.
           */
     512  inline bool get_joint_limits(  
           const std::string & joint_name,   const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,  
           SoftJointLimits & soft_limits )
          {
           return get_joint_limits(  
           joint_name,   lifecycle_node->get_node_parameters_interface(   ),  
           lifecycle_node->get_node_logging_interface(   ),   soft_limits );
          }
          
          } // namespace joint_limits
          
          #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_

ros2_control/joint_limits/test/joint_limits_rosparam_test.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #include <memory>
          
          #include "gtest/gtest.h"
          
          #include "joint_limits/joint_limits_rosparam.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          
      25  class JointLimitsRosParamTest : public ::testing::Test
          {
          public:
      28   void SetUp(   )
           {
           rclcpp::NodeOptions node_options;
           node_options.allow_undeclared_parameters(  true ).automatically_declare_parameters_from_overrides(  
           true );
          
           node_ = rclcpp::Node::make_shared(  "JointLimitsRosparamTestNode",   node_options );
           }
          
      37   void TearDown(   ) { node_.reset(   ); }
          
          protected:
      40   rclcpp::Node::SharedPtr node_;
          };
          
      43  TEST_F(  JointLimitsRosParamTest,   parse_joint_limits )
          {
           // Invalid specification
           {
           joint_limits::JointLimits limits;
          
           // test default values
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_TRUE(  std::isnan(  limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  limits.max_position ) );
           EXPECT_FALSE(  limits.has_velocity_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_velocity ) );
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_acceleration ) );
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_jerk ) );
           EXPECT_FALSE(  limits.has_effort_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_effort ) );
           EXPECT_FALSE(  limits.angle_wraparound );
          
           // try to read limits for not-existing joints
           EXPECT_FALSE(  get_joint_limits(  
           "bad_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
           EXPECT_FALSE(  get_joint_limits(  
           "unknown_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           // default values should not change
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_TRUE(  std::isnan(  limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  limits.max_position ) );
           EXPECT_FALSE(  limits.has_velocity_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_velocity ) );
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_acceleration ) );
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_jerk ) );
           EXPECT_FALSE(  limits.has_effort_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_effort ) );
           EXPECT_FALSE(  limits.angle_wraparound );
           }
          
           // Get full specification from parameter server
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "foo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_EQ(  0.0,   limits.min_position );
           EXPECT_EQ(  1.0,   limits.max_position );
          
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_EQ(  2.0,   limits.max_velocity );
          
           EXPECT_TRUE(  limits.has_acceleration_limits );
           EXPECT_EQ(  5.0,   limits.max_acceleration );
          
           EXPECT_TRUE(  limits.has_jerk_limits );
           EXPECT_EQ(  100.0,   limits.max_jerk );
          
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_EQ(  20.0,   limits.max_effort );
          
           // parameters is 'true',   but because there are position limits it is ignored
           EXPECT_FALSE(  limits.angle_wraparound );
           }
          
           // Specifying flags but not values should set nothing
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "yinfoo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_FALSE(  limits.has_velocity_limits );
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_FALSE(  limits.has_effort_limits );
           }
          
           // Specifying values but not flags should set nothing
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "yangfoo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_FALSE(  limits.has_velocity_limits );
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_FALSE(  limits.has_effort_limits );
           }
          
           // Disable already set values
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "foo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_TRUE(  limits.has_acceleration_limits );
           EXPECT_TRUE(  limits.has_jerk_limits );
           EXPECT_TRUE(  limits.has_effort_limits );
          
           EXPECT_TRUE(  get_joint_limits(  
           "antifoo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_FALSE(  limits.has_velocity_limits );
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_FALSE(  limits.has_effort_limits );
           EXPECT_TRUE(  limits.angle_wraparound );
           }
          
           // Incomplete position limits specification does not get loaded
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "baz_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_TRUE(  std::isnan(  limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  limits.max_position ) );
           }
          
           // Override only one field,   leave all others unchanged
           {
           joint_limits::JointLimits limits;
           EXPECT_TRUE(  get_joint_limits(  
           "bar_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           limits ) );
          
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_TRUE(  std::isnan(  limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  limits.max_position ) );
          
           // Max velocity is overridden
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_EQ(  2.0,   limits.max_velocity );
          
           EXPECT_FALSE(  limits.has_acceleration_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_acceleration ) );
          
           EXPECT_FALSE(  limits.has_jerk_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_jerk ) );
          
           EXPECT_FALSE(  limits.has_effort_limits );
           EXPECT_TRUE(  std::isnan(  limits.max_effort ) );
           }
          }
          
     202  TEST_F(  JointLimitsRosParamTest,   parse_soft_joint_limits )
          {
           // Invalid specification
           {
           joint_limits::SoftJointLimits soft_limits;
          
           // test default values
           EXPECT_TRUE(  std::isnan(  soft_limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.max_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_velocity ) );
          
           // try to read limits for not-existing joints
           EXPECT_FALSE(  get_joint_limits(  
           "bad_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           soft_limits ) );
           EXPECT_FALSE(  get_joint_limits(  
           "unknown_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           soft_limits ) );
          
           // default values should not change
           EXPECT_TRUE(  std::isnan(  soft_limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.max_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_velocity ) );
           }
          
           // Get full specification from parameter server
           {
           joint_limits::SoftJointLimits soft_limits;
           EXPECT_TRUE(  get_joint_limits(  
           "foo_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           soft_limits ) );
          
           EXPECT_EQ(  10.0,   soft_limits.k_position );
           EXPECT_EQ(  20.0,   soft_limits.k_velocity );
           EXPECT_EQ(  0.1,   soft_limits.min_position );
           EXPECT_EQ(  0.9,   soft_limits.max_position );
           }
          
           // Skip parsing soft limits if has_soft_limits is false
           {
           joint_limits::SoftJointLimits soft_limits;
           EXPECT_FALSE(  get_joint_limits(  
           "foobar_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           soft_limits ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.max_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_velocity ) );
           }
          
           // Incomplete soft limits specification does not get loaded
           {
           joint_limits::SoftJointLimits soft_limits;
           EXPECT_FALSE(  get_joint_limits(  
           "barbaz_joint",   node_->get_node_parameters_interface(   ),   node_->get_node_logging_interface(   ),  
           soft_limits ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.min_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.max_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_position ) );
           EXPECT_TRUE(  std::isnan(  soft_limits.k_velocity ) );
           }
          }
          
     267  class JointLimitsUndeclaredRosParamTest : public ::testing::Test
          {
          public:
     270   void SetUp(   ) { node_ = rclcpp::Node::make_shared(  "JointLimitsRosparamTestNode" ); }
          
     272   void TearDown(   ) { node_.reset(   ); }
          
          protected:
     275   rclcpp::Node::SharedPtr node_;
          };
          
     278  class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test
          {
          public:
     281   void SetUp(   )
           {
           lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared(  "JointLimitsRosparamTestNode" );
           }
          
     286   void TearDown(   ) { lifecycle_node_.reset(   ); }
          
          protected:
     289   rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_;
          };
          
     292  TEST_F(  JointLimitsUndeclaredRosParamTest,   parse_declared_joint_limits_node )
          {
           // Get full specification from parameter server - no need to test logic
           {
           joint_limits::JointLimits limits;
           // try to read limits for not-existing joints
           EXPECT_FALSE(  get_joint_limits(  "bad_joint",   node_,   limits ) );
           EXPECT_FALSE(  get_joint_limits(  "unknown_joint",   node_,   limits ) );
          
           // try to read existing but undeclared joint
           EXPECT_FALSE(  get_joint_limits(  "foo_joint",   node_,   limits ) );
          
           // declare parameters
           EXPECT_TRUE(  joint_limits::declare_parameters(  "foo_joint",   node_ ) );
          
           // now should be successful
           EXPECT_TRUE(  get_joint_limits(  "foo_joint",   node_,   limits ) );
          
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_EQ(  0.0,   limits.min_position );
           EXPECT_EQ(  1.0,   limits.max_position );
          
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_EQ(  2.0,   limits.max_velocity );
          
           EXPECT_TRUE(  limits.has_acceleration_limits );
           EXPECT_EQ(  5.0,   limits.max_acceleration );
          
           EXPECT_TRUE(  limits.has_jerk_limits );
           EXPECT_EQ(  100.0,   limits.max_jerk );
          
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_EQ(  20.0,   limits.max_effort );
          
           // parameters is 'true',   but because there are position limits it is ignored
           EXPECT_FALSE(  limits.angle_wraparound );
           }
          }
          
     331  TEST_F(  JointLimitsLifecycleNodeUndeclaredRosParamTest,   parse_declared_joint_limits_lifecycle_node )
          {
           // Get full specification from parameter server - no need to test logic
           {
           joint_limits::JointLimits limits;
           // try to read limits for not-existing joints
           EXPECT_FALSE(  get_joint_limits(  "bad_joint",   lifecycle_node_,   limits ) );
           EXPECT_FALSE(  get_joint_limits(  "unknown_joint",   lifecycle_node_,   limits ) );
          
           // try to read existing but undeclared joint
           EXPECT_FALSE(  get_joint_limits(  "foo_joint",   lifecycle_node_,   limits ) );
          
           // declare parameters
           EXPECT_TRUE(  joint_limits::declare_parameters(  "foo_joint",   lifecycle_node_ ) );
          
           // now should be successful
           EXPECT_TRUE(  get_joint_limits(  "foo_joint",   lifecycle_node_,   limits ) );
          
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_EQ(  0.0,   limits.min_position );
           EXPECT_EQ(  1.0,   limits.max_position );
          
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_EQ(  2.0,   limits.max_velocity );
          
           EXPECT_TRUE(  limits.has_acceleration_limits );
           EXPECT_EQ(  5.0,   limits.max_acceleration );
          
           EXPECT_TRUE(  limits.has_jerk_limits );
           EXPECT_EQ(  100.0,   limits.max_jerk );
          
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_EQ(  20.0,   limits.max_effort );
          
           // parameters is 'true',   but because there are position limits it is ignored
           EXPECT_FALSE(  limits.angle_wraparound );
           }
          }
          
     370  TEST_F(  JointLimitsUndeclaredRosParamTest,   parse_declared_soft_joint_limits_node )
          {
           // Get full specification from parameter server - no need to test logic
           {
           joint_limits::SoftJointLimits soft_limits;
           // try to read existing but undeclared joint
           EXPECT_FALSE(  get_joint_limits(  "foo_joint",   node_,   soft_limits ) );
          
           // declare parameters
           EXPECT_TRUE(  joint_limits::declare_parameters(  "foo_joint",   node_ ) );
          
           // now should be successful
           EXPECT_TRUE(  get_joint_limits(  "foo_joint",   node_,   soft_limits ) );
          
           EXPECT_EQ(  10.0,   soft_limits.k_position );
           EXPECT_EQ(  20.0,   soft_limits.k_velocity );
           EXPECT_EQ(  0.1,   soft_limits.min_position );
           EXPECT_EQ(  0.9,   soft_limits.max_position );
           }
          }
          
     391  TEST_F(  
           JointLimitsLifecycleNodeUndeclaredRosParamTest,   parse_declared_soft_joint_limits_lifecycle_node )
          {
           // Get full specification from parameter server - no need to test logic
           {
           joint_limits::SoftJointLimits soft_limits;
           // try to read existing but undeclared joint
           EXPECT_FALSE(  get_joint_limits(  "foo_joint",   lifecycle_node_,   soft_limits ) );
          
           // declare parameters
           EXPECT_TRUE(  joint_limits::declare_parameters(  "foo_joint",   lifecycle_node_ ) );
          
           // now should be successful
           EXPECT_TRUE(  get_joint_limits(  "foo_joint",   lifecycle_node_,   soft_limits ) );
          
           EXPECT_EQ(  10.0,   soft_limits.k_position );
           EXPECT_EQ(  20.0,   soft_limits.k_velocity );
           EXPECT_EQ(  0.1,   soft_limits.min_position );
           EXPECT_EQ(  0.9,   soft_limits.max_position );
           }
          }
          
     413  int main(  int argc,   char ** argv )
          {
           rclcpp::init(  argc,   argv );
           testing::InitGoogleTest(  &argc,   argv );
           int ret = RUN_ALL_TESTS(   );
           rclcpp::shutdown(   );
           return ret;
          }

ros2_control/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_
          #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_
          
          #include <hardware_interface/joint_handle.hpp>
          #include <hardware_interface/types/hardware_interface_type_values.hpp>
          
          #include <rclcpp/duration.hpp>
          #include <rclcpp/rclcpp.hpp>
          
          #include <rcppmath/clamp.hpp>
          
          #include <algorithm>
          #include <cassert>
          #include <cmath>
          #include <limits>
          #include <memory>
          #include <string>
          
          #include "joint_limits_interface/joint_limits.hpp"
          #include "joint_limits_interface/joint_limits_interface_exception.hpp"
          
          namespace joint_limits_interface
          {
          /**
           * The base class of limit handles for enforcing position,   velocity,   and effort limits of
           * an effort-controlled joint.
           */
      44  class JointLimitHandle
          {
          public:
           JointLimitHandle(   )
           : prev_pos_(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           prev_vel_(  0.0 ),  
           jposh_(  hardware_interface::HW_IF_POSITION ),  
           jvelh_(  hardware_interface::HW_IF_VELOCITY ),  
           jcmdh_(  "position_command" )
           {
           }
          
           JointLimitHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const JointLimits & limits )
           : jposh_(  jposh ),  
           jvelh_(  hardware_interface::HW_IF_VELOCITY ),  
           jcmdh_(  jcmdh ),  
           limits_(  limits ),  
           prev_pos_(  std::numeric_limits<double>::quiet_NaN(   ) ),  
      64   prev_vel_(  0.0 )
           {
           }
          
      68   JointLimitHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jvelh,  
           const hardware_interface::JointHandle & jcmdh,   const JointLimits & limits )
           : jposh_(  jposh ),  
           jvelh_(  jvelh ),  
           jcmdh_(  jcmdh ),  
           limits_(  limits ),  
           prev_pos_(  std::numeric_limits<double>::quiet_NaN(   ) ),  
           prev_vel_(  0.0 )
           {
           }
          
           /// \return Joint name.
           std::string get_name(   ) const
           {
           return jposh_ ? jposh_.get_name(   )
           : jvelh_ ? jvelh_.get_name(   )
           : jcmdh_ ? jcmdh_.get_name(   )
           : std::string(   );
           }
          
           /// Sub-class implementation of limit enforcing policy.
           virtual void enforce_limits(  const rclcpp::Duration & period ) = 0;
          
           /// Clear stored state,   causing it to reset next iteration.
           virtual void reset(   )
           {
           prev_pos_ = std::numeric_limits<double>::quiet_NaN(   );
           prev_vel_ = 0.0;
           }
          
          protected:
           hardware_interface::JointHandle jposh_;
           hardware_interface::JointHandle jvelh_;
           hardware_interface::JointHandle jcmdh_;
           joint_limits_interface::JointLimits limits_;
          
           // stored state - track position and velocity of last update
           double prev_pos_;
           double prev_vel_;
          
           /// Return velocity for limit calculations.
           /**
           * @param period Time since last measurement
           * @return the velocity,   from state if available,   otherwise from previous position history.
           */
           double get_velocity(  const rclcpp::Duration & period ) const
           {
           // if we have a handle to a velocity state we can directly return state velocity
           // otherwise we will estimate velocity from previous position (  command or state )
           return jvelh_ ? jvelh_.get_value(   ) : (  jposh_.get_value(   ) - prev_pos_ ) / period.seconds(   );
           }
          };
          
          /** The base class of limit handles for enforcing position,   velocity,   and effort limits of
           * an effort-controlled joint that has soft-limits.
           */
          class JointSoftLimitsHandle : public JointLimitHandle
          {
          public:
           JointSoftLimitsHandle(   ) {}
          
           JointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const JointLimits & limits,   const SoftJointLimits & soft_limits )
           : JointLimitHandle(  jposh,   jcmdh,   limits ),   soft_limits_(  soft_limits )
           {
           }
          
           JointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jvelh,  
           const hardware_interface::JointHandle & jcmdh,   const JointLimits & limits,  
           const SoftJointLimits & soft_limits )
           : JointLimitHandle(  jposh,   jvelh,   jcmdh,   limits ),   soft_limits_(  soft_limits )
           {
           }
          
          protected:
           joint_limits_interface::SoftJointLimits soft_limits_;
          };
          
          /** A handle used to enforce position and velocity limits of a position-controlled joint that does not have
           soft limits. */
          class PositionJointSaturationHandle : public JointLimitHandle
          {
          public:
           PositionJointSaturationHandle(   ) {}
          
           PositionJointSaturationHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const JointLimits & limits )
           : JointLimitHandle(  jposh,   jcmdh,   limits )
           {
           if (  limits_.has_position_limits )
           {
           min_pos_limit_ = limits_.min_position;
           max_pos_limit_ = limits_.max_position;
           }
           else
           {
           min_pos_limit_ = -std::numeric_limits<double>::max(   );
           max_pos_limit_ = std::numeric_limits<double>::max(   );
           }
           }
          
           /// Enforce position and velocity limits for a joint that is not subject to soft limits.
           /**
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period )
           {
           if (  std::isnan(  prev_pos_ ) )
           {
           prev_pos_ = jposh_.get_value(   );
           }
          
           double min_pos,   max_pos;
           if (  limits_.has_velocity_limits )
           {
           // enforce velocity limits
           // set constraints on where the position can be based on the
           // max velocity times seconds since last update
           const double delta_pos = limits_.max_velocity * period.seconds(   );
           min_pos = std::max(  prev_pos_ - delta_pos,   min_pos_limit_ );
           max_pos = std::min(  prev_pos_ + delta_pos,   max_pos_limit_ );
           }
           else
           {
           // no velocity limit,   so position is simply limited to set extents (  our imposed soft limits )
           min_pos = min_pos_limit_;
           max_pos = max_pos_limit_;
           }
          
           // clamp command position to our computed min/max position
           const double cmd = rcppmath::clamp(  jcmdh_.get_value(   ),   min_pos,   max_pos );
           jcmdh_.set_value(  cmd );
          
           prev_pos_ = cmd;
           }
          
          private:
           double min_pos_limit_,   max_pos_limit_;
          };
          
          /// A handle used to enforce position and velocity limits of a position-controlled joint.
          /**
           * This class implements a very simple position and velocity limits enforcing policy,   and tries to impose the least
           * amount of requisites on the underlying hardware platform.
           * This lowers considerably the entry barrier to use it,   but also implies some limitations.
           *
           * <b>Requisites</b>
           * - Position (  for non-continuous joints ) and velocity limits specification.
           * - Soft limits specification. The \c k_velocity parameter is \e not used.
           *
           * <b>Open loop nature</b>
           *
           * Joint position and velocity limits are enforced in an open-loop fashion,   that is,   the command is checked for
           * validity without relying on the actual position/velocity values.
           *
           * - Actual position values are \e not used because in some platforms there might be a substantial lag
           * between sending a command and executing it (  propagate command to hardware,   reach control objective,  
           * read from hardware ).
           *
           * - Actual velocity values are \e not used because of the above reason,   and because some platforms might not expose
           * trustworthy velocity measurements,   or none at all.
           *
           * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large
           * position tracking errors. Only the command is guaranteed to comply with the limits specification.
           *
           * \note: This handle type is \e stateful,   ie. it stores the previous position command to estimate the command
           * velocity.
           */
          
          // TODO(  anyone ): Leverage %Reflexxes Type II library for acceleration limits handling?
          class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle
          {
          public:
           PositionJointSoftLimitsHandle(   ) {}
          
           PositionJointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits,  
           const joint_limits_interface::SoftJointLimits & soft_limits )
           : JointSoftLimitsHandle(  jposh,   jcmdh,   limits,   soft_limits )
           {
           if (  !limits.has_velocity_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no velocity limits specification." );
           }
           }
          
           /// Enforce position and velocity limits for a joint subject to soft limits.
           /**
           * If the joint has no position limits (  eg. a continuous joint ),   only velocity limits will be
           * enforced.
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period ) override
           {
           assert(  period.seconds(   ) > 0.0 );
          
           // Current position
           if (  std::isnan(  prev_pos_ ) )
           {
           // Happens only once at initialization
           prev_pos_ = jposh_.get_value(   );
           }
           const double pos = prev_pos_;
          
           // Velocity bounds
           double soft_min_vel;
           double soft_max_vel;
          
           if (  limits_.has_position_limits )
           {
           // Velocity bounds depend on the velocity limit and the proximity to the position limit
           soft_min_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.min_position ),   -limits_.max_velocity,  
           limits_.max_velocity );
          
           soft_max_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.max_position ),   -limits_.max_velocity,  
           limits_.max_velocity );
           }
           else
           {
           // No position limits,   eg. continuous joints
           soft_min_vel = -limits_.max_velocity;
           soft_max_vel = limits_.max_velocity;
           }
          
           // Position bounds
           const double dt = period.seconds(   );
           double pos_low = pos + soft_min_vel * dt;
           double pos_high = pos + soft_max_vel * dt;
          
           if (  limits_.has_position_limits )
           {
           // This extra measure safeguards against pathological cases,   like when the soft limit lies
           // beyond the hard limit
           pos_low = std::max(  pos_low,   limits_.min_position );
           pos_high = std::min(  pos_high,   limits_.max_position );
           }
          
           // Saturate position command according to bounds
           const double pos_cmd = rcppmath::clamp(  jcmdh_.get_value(   ),   pos_low,   pos_high );
           jcmdh_.set_value(  pos_cmd );
          
           // Cache variables
           // todo: shouldn't this just be pos_cmd? why call into the command handle to
           // get what we have in the above line?
           prev_pos_ = jcmdh_.get_value(   );
           }
          };
          
          /**
           * A handle used to enforce position,   velocity,   and effort limits of an effort-controlled
           * joint that does not have soft limits.
           */
          class EffortJointSaturationHandle : public JointLimitHandle
          {
          public:
           EffortJointSaturationHandle(   ) {}
          
           EffortJointSaturationHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jvelh,  
           const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits )
           : JointLimitHandle(  jposh,   jvelh,   jcmdh,   limits )
           {
           if (  !limits.has_velocity_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no velocity limits specification." );
           }
           if (  !limits.has_effort_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no efforts limits specification." );
           }
           }
          
           EffortJointSaturationHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits )
           : EffortJointSaturationHandle(  
           jposh,   hardware_interface::JointHandle(  hardware_interface::HW_IF_VELOCITY ),   jcmdh,   limits )
           {
           }
          
           /**
           * Enforce position,   velocity,   and effort limits for a joint that is not subject
           * to soft limits.
           *
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period ) override
           {
           double min_eff = -limits_.max_effort;
           double max_eff = limits_.max_effort;
          
           if (  limits_.has_position_limits )
           {
           const double pos = jposh_.get_value(   );
           if (  pos < limits_.min_position )
           {
           min_eff = 0.0;
           }
           else if (  pos > limits_.max_position )
           {
           max_eff = 0.0;
           }
           }
          
           const double vel = get_velocity(  period );
           if (  vel < -limits_.max_velocity )
           {
           min_eff = 0.0;
           }
           else if (  vel > limits_.max_velocity )
           {
           max_eff = 0.0;
           }
          
           double clamped = rcppmath::clamp(  jcmdh_.get_value(   ),   min_eff,   max_eff );
           jcmdh_.set_value(  clamped );
           }
          };
          
          /// A handle used to enforce position,   velocity and effort limits of an effort-controlled joint.
          // TODO(  anyone ): This class is untested!. Update unit tests accordingly.
          class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle
          {
          public:
           EffortJointSoftLimitsHandle(   ) {}
          
           EffortJointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jvelh,  
           const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits,  
           const joint_limits_interface::SoftJointLimits & soft_limits )
           : JointSoftLimitsHandle(  jposh,   jvelh,   jcmdh,   limits,   soft_limits )
           {
           if (  !limits.has_velocity_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no velocity limits specification." );
           }
           if (  !limits.has_effort_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no effort limits specification." );
           }
           }
          
           EffortJointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits,  
           const joint_limits_interface::SoftJointLimits & soft_limits )
           : EffortJointSoftLimitsHandle(  
           jposh,   hardware_interface::JointHandle(  hardware_interface::HW_IF_VELOCITY ),   jcmdh,   limits,  
           soft_limits )
           {
           }
          
           /// Enforce position,   velocity and effort limits for a joint subject to soft limits.
           /**
           * If the joint has no position limits (  eg. a continuous joint ),   only velocity and effort limits
           * will be enforced.
           *
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period ) override
           {
           // Current state
           const double pos = jposh_.get_value(   );
           const double vel = get_velocity(  period );
          
           // Velocity bounds
           double soft_min_vel;
           double soft_max_vel;
          
           if (  limits_.has_position_limits )
           {
           // Velocity bounds depend on the velocity limit and the proximity to the position limit
           soft_min_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.min_position ),   -limits_.max_velocity,  
           limits_.max_velocity );
          
           soft_max_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.max_position ),   -limits_.max_velocity,  
           limits_.max_velocity );
           }
           else
           {
           // No position limits,   eg. continuous joints
           soft_min_vel = -limits_.max_velocity;
           soft_max_vel = limits_.max_velocity;
           }
          
           // Effort bounds depend on the velocity and effort bounds
           const double soft_min_eff = rcppmath::clamp(  
           -soft_limits_.k_velocity * (  vel - soft_min_vel ),   -limits_.max_effort,   limits_.max_effort );
          
           const double soft_max_eff = rcppmath::clamp(  
           -soft_limits_.k_velocity * (  vel - soft_max_vel ),   -limits_.max_effort,   limits_.max_effort );
          
           // Saturate effort command according to bounds
           const double eff_cmd = rcppmath::clamp(  jcmdh_.get_value(   ),   soft_min_eff,   soft_max_eff );
           jcmdh_.set_value(  eff_cmd );
           }
          };
          
          /// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint.
          class VelocityJointSaturationHandle : public JointLimitHandle
          {
          public:
           VelocityJointSaturationHandle(   ) {}
          
           VelocityJointSaturationHandle(  
           const hardware_interface::JointHandle & jvelh,   // currently unused
           const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits )
           : JointLimitHandle(  
           hardware_interface::JointHandle(  hardware_interface::HW_IF_POSITION ),   jvelh,   jcmdh,   limits )
           {
           if (  !limits.has_velocity_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no velocity limits specification." );
           }
           }
          
           VelocityJointSaturationHandle(  
           const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits )
           : JointLimitHandle(  
           hardware_interface::JointHandle(  hardware_interface::HW_IF_POSITION ),  
           hardware_interface::JointHandle(  hardware_interface::HW_IF_VELOCITY ),   jcmdh,   limits )
           {
           if (  !limits.has_velocity_limits )
           {
           throw joint_limits_interface::JointLimitsInterfaceException(  
           "Cannot enforce limits for joint '" + get_name(   ) +
           "'. It has no velocity limits specification." );
           }
           }
          
           /// Enforce joint velocity and acceleration limits.
           /**
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period ) override
           {
           // Velocity bounds
           double vel_low;
           double vel_high;
          
           if (  limits_.has_acceleration_limits )
           {
           assert(  period.seconds(   ) > 0.0 );
           const double dt = period.seconds(   );
          
           vel_low = std::max(  prev_vel_ - limits_.max_acceleration * dt,   -limits_.max_velocity );
           vel_high = std::min(  prev_vel_ + limits_.max_acceleration * dt,   limits_.max_velocity );
           }
           else
           {
           vel_low = -limits_.max_velocity;
           vel_high = limits_.max_velocity;
           }
          
           // Saturate velocity command according to limits
           const double vel_cmd = rcppmath::clamp(  jcmdh_.get_value(   ),   vel_low,   vel_high );
           jcmdh_.set_value(  vel_cmd );
          
           // Cache variables
           prev_vel_ = jcmdh_.get_value(   );
           }
          };
          
          /**
           * A handle used to enforce position,   velocity,   and acceleration limits of a
           * velocity-controlled joint.
           */
          class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle
          {
          public:
           VelocityJointSoftLimitsHandle(   ) {}
          
           VelocityJointSoftLimitsHandle(  
           const hardware_interface::JointHandle & jposh,   const hardware_interface::JointHandle & jvelh,  
           const hardware_interface::JointHandle & jcmdh,  
           const joint_limits_interface::JointLimits & limits,  
           const joint_limits_interface::SoftJointLimits & soft_limits )
           : JointSoftLimitsHandle(  jposh,   jvelh,   jcmdh,   limits,   soft_limits )
           {
           if (  limits.has_velocity_limits )
           {
           max_vel_limit_ = limits.max_velocity;
           }
           else
           {
           max_vel_limit_ = std::numeric_limits<double>::max(   );
           }
           }
          
           /**
           * Enforce position,   velocity,   and acceleration limits for a velocity-controlled joint
           * subject to soft limits.
           *
           * \param[in] period Control period.
           */
           void enforce_limits(  const rclcpp::Duration & period )
           {
           double min_vel,   max_vel;
           if (  limits_.has_position_limits )
           {
           // Velocity bounds depend on the velocity limit and the proximity to the position limit.
           const double pos = jposh_.get_value(   );
           min_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.min_position ),   -max_vel_limit_,  
           max_vel_limit_ );
           max_vel = rcppmath::clamp(  
           -soft_limits_.k_position * (  pos - soft_limits_.max_position ),   -max_vel_limit_,  
           max_vel_limit_ );
           }
           else
           {
           min_vel = -max_vel_limit_;
           max_vel = max_vel_limit_;
           }
          
           if (  limits_.has_acceleration_limits )
           {
           const double vel = get_velocity(  period );
           const double delta_t = period.seconds(   );
           min_vel = std::max(  vel - limits_.max_acceleration * delta_t,   min_vel );
           max_vel = std::min(  vel + limits_.max_acceleration * delta_t,   max_vel );
           }
          
           jcmdh_.set_value(  rcppmath::clamp(  jcmdh_.get_value(   ),   min_vel,   max_vel ) );
           }
          
          private:
           double max_vel_limit_;
          };
          
          // TODO(  anyone ): Port this to ROS 2
          // //**
          // * Interface for enforcing joint limits.
          // *
          // * \tparam HandleType %Handle type. Must implement the following methods:
          // * \code
          // * void enforce_limits(   );
          // * std::string get_name(   ) const;
          // * \endcode
          // */
          // template<class HandleType>
          // class joint_limits_interface::JointLimitsInterface
          // : public hardware_interface::ResourceManager<HandleType>
          // {
          // public:
          // HandleType getHandle(  const std::string & name )
          // {
          // // Rethrow exception with a meaningful type
          // try {
          // return this->hardware_interface::ResourceManager<HandleType>::getHandle(  name );
          // } catch (  const std::logic_error & e ) {
          // throw joint_limits_interface::JointLimitsInterfaceException(  e.what(   ) );
          // }
          // }
          //
          // /** \name Real-Time Safe Functions
          // *\{*/
          // /** Enforce limits for all managed handles. */
          // void enforce_limits(  const rclcpp::Duration & period )
          // {
          // for (  auto && resource_name_and_handle : this->resource_map_ ) {
          // resource_name_and_handle.second.enforce_limits(  period );
          // }
          // }
          // /*\}*/
          // };
          //
          // /** Interface for enforcing limits on a position-controlled joint through saturation. */
          // class PositionJointSaturationInterface
          // : public joint_limits_interface::JointLimitsInterface<PositionJointSaturationHandle>
          // {
          // public:
          // /** \name Real-Time Safe Functions
          // *\{*/
          // /** Reset all managed handles. */
          // void reset(   )
          // {
          // for (  auto && resource_name_and_handle : this->resource_map_ ) {
          // resource_name_and_handle.second.reset(   );
          // }
          // }
          // /*\}*/
          // };
          //
          // /** Interface for enforcing limits on a position-controlled joint with soft position limits. */
          // class PositionJointSoftLimitsInterface
          // : public joint_limits_interface::JointLimitsInterface<PositionJointSoftLimitsHandle>
          // {
          // public:
          // /** \name Real-Time Safe Functions
          // *\{*/
          // /** Reset all managed handles. */
          // void reset(   )
          // {
          // for (  auto && resource_name_and_handle : this->resource_map_ ) {
          // resource_name_and_handle.second.reset(   );
          // }
          // }
          // /*\}*/
          // };
          //
          // /** Interface for enforcing limits on an effort-controlled joint through saturation. */
          // class EffortJointSaturationInterface
          // : public joint_limits_interface::JointLimitsInterface<EffortJointSaturationHandle>
          // {
          // };
          //
          // /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */
          // class EffortJointSoftLimitsInterface
          // : public joint_limits_interface::JointLimitsInterface<EffortJointSoftLimitsHandle>
          // {
          // };
          //
          // /** Interface for enforcing limits on a velocity-controlled joint through saturation. */
          // class VelocityJointSaturationInterface
          // : public joint_limits_interface::JointLimitsInterface<VelocityJointSaturationHandle>
          // {
          // };
          //
          // /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */
          // class VelocityJointSoftLimitsInterface
          // : public joint_limits_interface::JointLimitsInterface<VelocityJointSoftLimitsHandle>
          // {
          // };
          } // namespace joint_limits_interface
          
          #endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_

ros2_control/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_
          #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_
          
          #include <string>
          
          namespace joint_limits_interface
          {
          /// An exception related to a \ref JointLimitsInterface
      23  class JointLimitsInterfaceException : public std::exception
          {
          public:
      26   explicit JointLimitsInterfaceException(  const std::string & message ) : msg(  message ) {}
          
           const char * what(   ) const noexcept override { return msg.c_str(   ); }
          
          private:
           std::string msg;
          };
          
          } // namespace joint_limits_interface
          
          #endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_

ros2_control/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_
          #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_
          
          #include <urdf/urdfdom_compatibility.h>
          #include <urdf_model/joint.h>
          #include <joint_limits_interface/joint_limits.hpp>
          #include <rclcpp/rclcpp.hpp>
          
          namespace joint_limits_interface
          {
          /**
           * Populate a JointLimits instance from URDF joint data.
           * \param[in] urdf_joint URDF joint.
           * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing
           * values. Values in \e limits not present in \e urdf_joint remain unchanged.
           * \return True if \e urdf_joint has a valid limits specification,   false otherwise.
           */
      34  inline bool getJointLimits(  urdf::JointConstSharedPtr urdf_joint,   JointLimits & limits )
          {
           if (  !urdf_joint || !urdf_joint->limits )
           {
           return false;
           }
          
           limits.has_position_limits =
           urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC;
           if (  limits.has_position_limits )
           {
           limits.min_position = urdf_joint->limits->lower;
           limits.max_position = urdf_joint->limits->upper;
           }
          
           if (  !limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS )
           {
           limits.angle_wraparound = true;
           }
          
           limits.has_velocity_limits = true;
           limits.max_velocity = urdf_joint->limits->velocity;
          
           limits.has_acceleration_limits = false;
          
           limits.has_effort_limits = true;
           limits.max_effort = urdf_joint->limits->effort;
          
           return true;
          }
          
          /**
           * Populate a SoftJointLimits instance from URDF joint data.
           * \param[in] urdf_joint URDF joint.
           * \param[out] soft_limits Where URDF soft joint limit data gets written into.
           * \return True if \e urdf_joint has a valid soft limits specification,   false otherwise.
           */
      71  inline bool getSoftJointLimits(  urdf::JointConstSharedPtr urdf_joint,   SoftJointLimits & soft_limits )
          {
           if (  !urdf_joint || !urdf_joint->safety )
           {
           return false;
           }
          
           soft_limits.min_position = urdf_joint->safety->soft_lower_limit;
           soft_limits.max_position = urdf_joint->safety->soft_upper_limit;
           soft_limits.k_position = urdf_joint->safety->k_position;
           soft_limits.k_velocity = urdf_joint->safety->k_velocity;
          
           return true;
          }
          
          } // namespace joint_limits_interface
          
          #endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_

ros2_control/joint_limits_interface/test/joint_limits_urdf_test.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #include <gtest/gtest.h>
          
          #include <joint_limits_interface/joint_limits_urdf.hpp>
          
          #include <string>
          
      23  class JointLimitsUrdfTest : public ::testing::Test
          {
          public:
      26   JointLimitsUrdfTest(   )
           {
           urdf_limits.reset(  new urdf::JointLimits );
           urdf_limits->effort = 8.0;
           urdf_limits->velocity = 2.0;
           urdf_limits->lower = -1.0;
           urdf_limits->upper = 1.0;
          
           urdf_safety.reset(  new urdf::JointSafety );
           urdf_safety->k_position = 20.0;
           urdf_safety->k_velocity = 40.0;
           urdf_safety->soft_lower_limit = -0.8;
           urdf_safety->soft_upper_limit = 0.8;
          
           urdf_joint.reset(  new urdf::Joint );
           urdf_joint->limits = urdf_limits;
           urdf_joint->safety = urdf_safety;
          
           urdf_joint->type = urdf::Joint::UNKNOWN;
           }
          
          protected:
      48   urdf::JointLimitsSharedPtr urdf_limits;
      49   urdf::JointSafetySharedPtr urdf_safety;
      50   urdf::JointSharedPtr urdf_joint;
          };
          
      53  TEST_F(  JointLimitsUrdfTest,   GetJointLimits )
          {
           // Unset URDF joint
           {
           joint_limits_interface::JointLimits limits;
           urdf::JointSharedPtr urdf_joint_bad;
           EXPECT_FALSE(  getJointLimits(  urdf_joint_bad,   limits ) );
           }
          
           // Unset URDF limits
           {
           joint_limits_interface::JointLimits limits;
           urdf::JointSharedPtr urdf_joint_bad(  new urdf::Joint );
           EXPECT_FALSE(  getJointLimits(  urdf_joint_bad,   limits ) );
           }
          
           // Valid URDF joint,   CONTINUOUS type
           {
           urdf_joint->type = urdf::Joint::CONTINUOUS;
          
           joint_limits_interface::JointLimits limits;
           EXPECT_TRUE(  getJointLimits(  urdf_joint,   limits ) );
          
           // Position
           EXPECT_FALSE(  limits.has_position_limits );
           EXPECT_TRUE(  limits.angle_wraparound );
          
           // Velocity
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->velocity,   limits.max_velocity );
          
           // Acceleration
           EXPECT_FALSE(  limits.has_acceleration_limits );
          
           // Effort
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->effort,   limits.max_effort );
           }
          
           // Valid URDF joint,   REVOLUTE type
           {
           urdf_joint->type = urdf::Joint::REVOLUTE;
          
           joint_limits_interface::JointLimits limits;
           EXPECT_TRUE(  getJointLimits(  urdf_joint,   limits ) );
          
           // Position
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->lower,   limits.min_position );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->upper,   limits.max_position );
           EXPECT_FALSE(  limits.angle_wraparound );
          
           // Velocity
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->velocity,   limits.max_velocity );
          
           // Acceleration
           EXPECT_FALSE(  limits.has_acceleration_limits );
          
           // Effort
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->effort,   limits.max_effort );
           }
          
           // Valid URDF joint,   PRISMATIC type
           {
           urdf_joint->type = urdf::Joint::PRISMATIC;
          
           joint_limits_interface::JointLimits limits;
           EXPECT_TRUE(  getJointLimits(  urdf_joint,   limits ) );
          
           // Position
           EXPECT_TRUE(  limits.has_position_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->lower,   limits.min_position );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->upper,   limits.max_position );
           EXPECT_FALSE(  limits.angle_wraparound );
          
           // Velocity
           EXPECT_TRUE(  limits.has_velocity_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->velocity,   limits.max_velocity );
          
           // Acceleration
           EXPECT_FALSE(  limits.has_acceleration_limits );
          
           // Effort
           EXPECT_TRUE(  limits.has_effort_limits );
           EXPECT_DOUBLE_EQ(  urdf_joint->limits->effort,   limits.max_effort );
           }
          }
          
     143  TEST_F(  JointLimitsUrdfTest,   GetSoftJointLimits )
          {
           // Unset URDF joint
           {
           joint_limits_interface::SoftJointLimits soft_limits;
           urdf::JointSharedPtr urdf_joint_bad;
           EXPECT_FALSE(  getSoftJointLimits(  urdf_joint_bad,   soft_limits ) );
           }
          
           // Unset URDF limits
           {
           joint_limits_interface::SoftJointLimits soft_limits;
           urdf::JointSharedPtr urdf_joint_bad(  new urdf::Joint );
           EXPECT_FALSE(  getSoftJointLimits(  urdf_joint_bad,   soft_limits ) );
           }
          
           // Valid URDF joint
           {
           joint_limits_interface::SoftJointLimits soft_limits;
           EXPECT_TRUE(  getSoftJointLimits(  urdf_joint,   soft_limits ) );
          
           // Soft limits
           EXPECT_DOUBLE_EQ(  urdf_joint->safety->soft_lower_limit,   soft_limits.min_position );
           EXPECT_DOUBLE_EQ(  urdf_joint->safety->soft_upper_limit,   soft_limits.max_position );
           EXPECT_DOUBLE_EQ(  urdf_joint->safety->k_position,   soft_limits.k_position );
           EXPECT_DOUBLE_EQ(  urdf_joint->safety->k_velocity,   soft_limits.k_velocity );
           }
          }
          
     172  int main(  int argc,   char ** argv )
          {
           testing::InitGoogleTest(  &argc,   argv );
           return RUN_ALL_TESTS(   );
          }

ros2_control/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp

          // Copyright (  c ) 2021 Stogl Robotics Consulting
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_
          #define ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_
          
          #include <string>
          
          namespace ros2_control_test_assets
          {
          // 1. Industrial Robots with only one interface
      23  const auto valid_urdf_ros2_control_system_one_interface =
           R"(  
           <ros2_control name="RRBotSystemPositionOnly" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           )";
          
          // 2. Industrial Robots with multiple interfaces (  cannot be written at the same time )
          // Note,   joint parameters can be any string
          const auto valid_urdf_ros2_control_system_multi_interface =
           R"(  
           <ros2_control name="RRBotSystemMultiInterface" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           <param name="initial_value">1.2</param>
           </command_interface>
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           <param name="initial_value">3.4</param>
           </command_interface>
           <command_interface name="effort">
           <param name="min">-0.5</param>
           <param name="max">0.5"</param>
           </command_interface>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="effort"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="effort"/>
           </joint>
           </ros2_control>
           )";
          
          // 3. Industrial Robots with integrated sensor
          const auto valid_urdf_ros2_control_system_robot_with_sensor =
           R"(  
           <ros2_control name="RRBotSystemWithSensor" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemWithSensorHardware</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <sensor name="tcp_fts_sensor">
           <state_interface name="fx"/>
           <state_interface name="fy"/>
           <state_interface name="fz"/>
           <state_interface name="tx"/>
           <state_interface name="ty"/>
           <state_interface name="tz"/>
           <param name="frame_id">kuka_tcp</param>
           <param name="lower_limits">-100</param>
           <param name="upper_limits">100</param>
           </sensor>
           </ros2_control>
           )";
          
          // 4. Industrial Robots with externally connected sensor
          const auto valid_urdf_ros2_control_system_robot_with_external_sensor =
           R"(  
           <ros2_control name="RRBotSystemPositionOnlyWithExternalSensor" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotForceTorqueSensor2D" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
           <param name="example_param_read_for_sec">0.43</param>
           </hardware>
           <sensor name="tcp_fts_sensor">
           <state_interface name="fx"/>
           <state_interface name="fy"/>
           <state_interface name="fz"/>
           <state_interface name="tx"/>
           <state_interface name="ty"/>
           <state_interface name="tz"/>
           <param name="frame_id">kuka_tcp</param>
           <param name="lower_limits">-100</param>
           <param name="upper_limits">100</param>
           </sensor>
           </ros2_control>
           )";
          
          // 5. Modular Robots with separate communication to each actuator
          const auto valid_urdf_ros2_control_actuator_modular_robot =
           R"(  
           <ros2_control name="RRBotModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotModularJoint2" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           )";
          
          // 6. Modular Robots with actuators not providing states and with additional sensors
          // Example for simple transmission
          const auto valid_urdf_ros2_control_actuator_modular_robot_sensors =
           R"(  
           <ros2_control name="RRBotModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTansmission</plugin>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>325.949</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           <ros2_control name="RRBotModularJoint2" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint2">
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           )";
          
          // 7. Modular Robots with separate communication to each "actuator" with multi joints
          // Example for complex transmission (  multi-joint/multi-actuator ) - (  system component is used )
          const auto valid_urdf_ros2_control_system_multi_joints_transmission =
           R"(  
           <ros2_control name="RRBotModularWrist" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/ActuatorHardwareMultiDOF</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1"/>
           <actuator name="joint2_motor" role="actuator2"/>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>10</mechanical_reduction>
           <offset>0.5</offset>
           </joint>
           <joint name="joint2" role="joint2">
           <mechanical_reduction>50</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           )";
          
          // 8. Sensor only
          const auto valid_urdf_ros2_control_sensor_only =
           R"(  
           <ros2_control name="CameraWithIMU" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/CameraWithIMUSensor</plugin>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <sensor name="sensor1">
           <state_interface name="roll"/>
           <state_interface name="pitch"/>
           <state_interface name="yaw"/>
           </sensor>
           <sensor name="sensor2">
           <state_interface name="image"/>
           </sensor>
           </ros2_control>
           )";
          
          // 9. Actuator Only
          const auto valid_urdf_ros2_control_actuator_only =
           R"(  
           <ros2_control name="ActuatorModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.13</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/RotationToLinerTansmission</plugin>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>325.949</mechanical_reduction>
           </joint>
           <param name="additional_special_parameter">1337</param>
           </transmission>
           </ros2_control>
           )";
          
          // 10. Industrial Robots with integrated GPIO
          const auto valid_urdf_ros2_control_system_robot_with_gpio =
           R"(  
           <ros2_control name="RRBotSystemWithGPIO" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemWithGPIOHardware</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <gpio name="flange_analog_IOs">
           <command_interface name="analog_output1"/>
           <state_interface name="analog_output1"/> <!-- Needed to know current state of the output -->
           <state_interface name="analog_input1"/>
           <state_interface name="analog_input2"/>
           </gpio>
           <gpio name="flange_vacuum">
           <command_interface name="vacuum"/>
           <state_interface name="vacuum"/> <!-- Needed to know current state of the input -->
           </gpio>
           </ros2_control>
           )";
          
          // 11. Industrial Robots using size and data_type attributes
          const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type =
           R"(  
           <ros2_control name="RRBotSystemWithSizeAndDataType" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="position"/>
           </joint>
           <gpio name="flange_IOS">
           <command_interface name="digital_output" size="2" data_type="bool"/>
           <state_interface name="analog_input" size="3"/>
           <state_interface name="image" data_type="cv::Mat"/>
           </gpio>
           </ros2_control>
           )";
          
          // Errors
          const auto invalid_urdf_ros2_control_invalid_child =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardwarert>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardwarert>
           </ros2_control>
            )";
          
          const auto invalid_urdf_ros2_control_missing_attribute =
           R"(  
           <ros2_control type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           </ros2_control>
            )";
          
          const auto invalid_urdf_ros2_control_component_missing_class_type =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <param name="min_position_value">-1</param>
           <param name="max_position_value">1</param>
           </joint>
           </ros2_control>
           )";
          
          const auto invalid_urdf_ros2_control_parameter_missing_name =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param>2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <plugin>ros2_control_components/PositionJoint</plugin>
           <param name="min_position_value">-1</param>
           <param name="max_position_value">1</param>
           </joint>
           </ros2_control>
           )";
          
          const auto invalid_urdf_ros2_control_component_class_type_empty =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <plugin></plugin>
           <param name="min_position_value">-1</param>
           <param name="max_position_value">1</param>
           </joint>
           </ros2_control>
           )";
          
          const auto invalid_urdf_ros2_control_component_interface_type_empty =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <plugin>ros2_control_components/PositionJoint</plugin>
           <state_interface></state_interface>
           <param name="min_position_value">-1</param>
           <param name="max_position_value">1</param>
           </joint>
           </ros2_control>
           )";
          
          const auto invalid_urdf_ros2_control_parameter_empty =
           R"(  
           <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
           <param name="example_param_write_for_sec"></param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="position">
           <param name="min_position_value">-1</param>
           <param name="max_position_value">1</param>
           </command_interface>
           </joint>
           </ros2_control>
           )";
          
          const auto invalid_urdf2_ros2_control_illegal_size =
           R"(  
           <ros2_control name="RRBotSystemWithIllegalSize" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemWithIllegalSize</plugin>
           </hardware>
           <gpio name="flange_IOS">
           <command_interface name="digital_output" data_type="bool" size="-4"/>
           </gpio>
           </ros2_control>
           )";
          
          const auto invalid_urdf2_ros2_control_illegal_size2 =
           R"(  
           <ros2_control name="RRBotSystemWithIllegalSize2" type="system">
           <hardware>
           <plugin>ros2_control_demo_hardware/RRBotSystemWithIllegalSize2</plugin>
           </hardware>
           <gpio name="flange_IOS">
           <command_interface name="digital_output" data_type="bool" size="ILLEGAL"/>
           </gpio>
           </ros2_control>
           )";
          
          const auto invalid_urdf2_hw_transmission_joint_mismatch =
           R"(  
           <ros2_control name="ActuatorModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint31415" role="joint1"/>
           </transmission>
           </ros2_control>
           )";
          
          const auto invalid_urdf2_transmission_given_too_many_joints =
           R"(  
           <ros2_control name="ActuatorModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint1" role="joint1"/>
           <joint name="joint2" role="joint2"/>
           </transmission>
           </ros2_control>
           )";
          
          } // namespace ros2_control_test_assets
          
          #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_

ros2_control/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

          // Copyright 2020 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
          #define ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
          
          #include <string>
          #include <vector>
          
          namespace ros2_control_test_assets
          {
      23  const auto urdf_head =
           R"(  
          <?xml version="1.0" encoding="utf-8"?>
          <!-- =================================================================================== -->
          <!-- | This document was autogenerated by xacro from minimal_robot.urdf.xacro | -->
          <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
          <!-- =================================================================================== -->
          <robot name="MinimalRobot">
           <!-- Used for fixing robot -->
           <link name="world"/>
           <joint name="base_joint" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <parent link="world"/>
           <child link="base_link"/>
           </joint>
           <link name="base_link">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.2" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
           <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="joint1" type="revolute">
           <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
           <parent link="base_link"/>
           <child link="link1"/>
           <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
           </joint>
           <link name="link1">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
           <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="joint2" type="revolute">
           <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
           <parent link="link1"/>
           <child link="link2"/>
           <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
           </joint>
           <link name="link2">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
           <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="tool_joint" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 1"/>
           <parent link="link2"/>
           <child link="tool_link"/>
           </joint>
           <link name="tool_link">
           </link>
           )";
          
          const auto urdf_tail =
           R"(  
          </robot>
           )";
          
          const auto hardware_resources =
           R"(  
           <ros2_control name="TestActuatorHardware" type="actuator">
           <hardware>
           <plugin>test_actuator</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <command_interface name="max_velocity" />
           </joint>
           </ros2_control>
           <ros2_control name="TestSensorHardware" type="sensor">
           <hardware>
           <plugin>test_sensor</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <sensor name="sensor1">
           <state_interface name="velocity"/>
           </sensor>
           </ros2_control>
           <ros2_control name="TestSystemHardware" type="system">
           <hardware>
           <plugin>test_system</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="acceleration"/>
           <command_interface name="max_acceleration" />
           </joint>
           <joint name="joint3">
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           <state_interface name="acceleration"/>
           </joint>
           <joint name="configuration">
           <command_interface name="max_tcp_jerk"/>
           <state_interface name="max_tcp_jerk"/>
           </joint>
           </ros2_control>
           )";
          
          const auto hardware_resources_missing_state_keys =
           R"(  
           <ros2_control name="TestActuatorHardware" type="actuator">
           <hardware>
           <plugin>test_actuator</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           <ros2_control name="TestSensorHardware" type="sensor">
           <hardware>
           <plugin>test_sensor</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <sensor name="sensor1">
           <state_interface name="velocity"/>
           <state_interface name="does_not_exist"/>
           </sensor>
           </ros2_control>
           <ros2_control name="TestSystemHardware" type="system">
           <hardware>
           <plugin>test_system</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="does_not_exist"/>
           </joint>
           <joint name="joint3">
           <command_interface name="velocity"/>
           <state_interface name="position"/>
           <state_interface name="does_not_exist"/>
           </joint>
           </ros2_control>
           )";
          
          const auto hardware_resources_missing_command_keys =
           R"(  
           <ros2_control name="TestActuatorHardware" type="actuator">
           <hardware>
           <plugin>test_actuator</plugin>
           </hardware>
           <joint name="joint1">
           <command_interface name="position"/>
           <command_interface name="does_not_exist"/>
           <state_interface name="position"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           <ros2_control name="TestSensorHardware" type="sensor">
           <hardware>
           <plugin>test_sensor</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <sensor name="sensor1">
           <state_interface name="velocity"/>
           </sensor>
           </ros2_control>
           <ros2_control name="TestSystemHardware" type="system">
           <hardware>
           <plugin>test_system</plugin>
           <param name="example_param_write_for_sec">2</param>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="velocity"/>
           <command_interface name="does_not_exist"/>
           <state_interface name="position"/>
           </joint>
           <joint name="joint3">
           <command_interface name="velocity"/>
           <command_interface name="does_not_exist"/>
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           )";
          
          const auto diffbot_urdf =
           R"(  
          <?xml version="1.0" ?>
          <robot name="robot">
           <!-- Space btw top of beam and the each joint -->
           <!-- Links: inertial,  visual,  collision -->
           <link name="base_link">
           <inertial>
           <!-- origin is relative -->
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <mass value="5"/>
           <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
           </inertial>
           <visual>
           <geometry>
           <box size="0.5 0.1 0.1"/>
           </geometry>
           </visual>
           <collision>
           <!-- origin is relative -->
           <origin xyz="0 0 0"/>
           <geometry>
           <box size="0.5 0.1 0.1"/>
           </geometry>
           </collision>
           </link>
           <link name="base_footprint">
           <visual>
           <geometry>
           <sphere radius="0.01"/>
           </geometry>
           </visual>
           <collision>
           <origin xyz="0 0 0"/>
           <geometry>
           <sphere radius="0.00000001"/>
           </geometry>
           </collision>
           </link>
           <joint name="base_footprint_joint" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 0.11"/>
           <child link="base_link"/>
           <parent link="base_footprint"/>
           </joint>
           <link name="wheel_0_link">
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <mass value="1"/>
           <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.086" radius="0.11"/>
           </geometry>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.086" radius="0.11"/>
           </geometry>
           </collision>
           </link>
           <joint name="wheel_0_joint" type="continuous">
           <parent link="base_link"/>
           <child link="wheel_0_link"/>
           <origin rpy="-1.5707963267948966 0 0" xyz="0.3 0 0"/>
           <axis xyz="0 0 1"/>
           </joint>
           <!-- Transmission is important to link the joints and the controller -->
           <transmission name="wheel_0_joint_trans" type="SimpleTransmission">
           <actuator name="wheel_0_joint_motor"/>
           <joint name="wheel_0_joint"/>
           <mechanicalReduction>1</mechanicalReduction>
           <motorTorqueConstant>1</motorTorqueConstant>
           </transmission>
           <gazebo reference="wheel_0_link">
           <material>Gazebo/Red</material>
           </gazebo>
           <link name="wheel_1_link">
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <mass value="1"/>
           <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.086" radius="0.11"/>
           </geometry>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.086" radius="0.11"/>
           </geometry>
           </collision>
           </link>
           <joint name="wheel_1_joint" type="continuous">
           <parent link="base_link"/>
           <child link="wheel_1_link"/>
           <origin rpy="-1.5707963267948966 0 0" xyz="-0.2 0 0"/>
           <axis xyz="0 0 1"/>
           </joint>
           <!-- Transmission is important to link the joints and the controller -->
           <transmission name="wheel_1_joint_trans" type="SimpleTransmission">
           <actuator name="wheel_1_joint_motor"/>
           <joint name="wheel_1_joint"/>
           <mechanicalReduction>1</mechanicalReduction>
           <motorTorqueConstant>1</motorTorqueConstant>
           </transmission>
           <gazebo reference="wheel_1_link">
           <material>Gazebo/Red</material>
           </gazebo>
           <!-- Colour -->
           <gazebo reference="base_link">
           <material>Gazebo/Green</material>
           </gazebo>
           <gazebo reference="base_footprint">
           <material>Gazebo/Purple</material>
           </gazebo>
           <ros2_control name="TestActuatorHardwareLeft" type="actuator">
           <hardware>
           <plugin>test_actuator</plugin>
           </hardware>
           <joint name="wheel_left">
           <state_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           <ros2_control name="TestActuatorHardwareRight" type="actuator">
           <hardware>
           <plugin>test_actuator</plugin>
           </hardware>
           <joint name="wheel_right">
           <state_interface name="position"/>
           <command_interface name="velocity"/>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
          </robot>
           )";
          
          const auto minimal_robot_urdf =
           std::string(  urdf_head ) + std::string(  hardware_resources ) + std::string(  urdf_tail );
          
          const auto minimal_robot_missing_state_keys_urdf =
           std::string(  urdf_head ) + std::string(  hardware_resources_missing_state_keys ) +
           std::string(  urdf_tail );
          
          const auto minimal_robot_missing_command_keys_urdf =
           std::string(  urdf_head ) + std::string(  hardware_resources_missing_command_keys ) +
           std::string(  urdf_tail );
          
          [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_NAME = "TestActuatorHardware";
          [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_TYPE = "actuator";
          [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_CLASS_TYPE = "test_actuator";
          [[maybe_unused]] const std::vector<std::string> TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = {
           "joint1/position",   "joint1/max_velocity"};
          [[maybe_unused]] const std::vector<std::string> TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = {
           "joint1/position",   "joint1/velocity",   "joint1/some_unlisted_interface"};
          
          [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_NAME = "TestSensorHardware";
          [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_TYPE = "sensor";
          [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_CLASS_TYPE = "test_sensor";
          [[maybe_unused]] const std::vector<std::string> TEST_SENSOR_HARDWARE_COMMAND_INTERFACES = {""};
          [[maybe_unused]] const std::vector<std::string> TEST_SENSOR_HARDWARE_STATE_INTERFACES = {
           "sensor1/velocity"};
          
          [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_NAME = "TestSystemHardware";
          [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system";
          [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_CLASS_TYPE = "test_system";
          [[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = {
           "joint2/velocity",   "joint2/max_acceleration",   "joint3/velocity",   "configuration/max_tcp_jerk"};
          [[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_STATE_INTERFACES = {
           "joint2/position",   "joint2/velocity",   "joint2/acceleration",   "joint3/position",  
           "joint3/velocity",   "joint3/acceleration",   "configuration/max_tcp_jerk"};
          
          } // namespace ros2_control_test_assets
          
          #endif // ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_

ros2_control/transmission_interface/include/transmission_interface/accessor.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__ACCESSOR_HPP_
          #define TRANSMISSION_INTERFACE__ACCESSOR_HPP_
          
          #include <algorithm>
          #include <set>
          #include <string>
          #include <vector>
          
          namespace transmission_interface
          {
          template <typename T>
      26  std::string to_string(  const std::vector<T> & list )
          {
           std::stringstream ss;
           ss << "[";
           for (  const auto & elem : list )
           {
           ss << elem << ",   ";
           }
          
           if (  !list.empty(   ) )
           {
           ss.seekp(  -2,   std::ios_base::end ); // remove last ",   "
           }
           ss << "]";
           return ss.str(   );
          }
          
          template <class T>
      44  std::vector<std::string> get_names(  const std::vector<T> & handles )
          {
           std::set<std::string> names;
           std::transform(  
           handles.cbegin(   ),   handles.cend(   ),   std::inserter(  names,   names.end(   ) ),  
           [](  const auto & handle ) { return handle.get_name(   ); } );
           return std::vector<std::string>(  names.begin(   ),   names.end(   ) );
          }
          
          template <typename T>
      54  std::vector<T> get_ordered_handles(  
           const std::vector<T> & unordered_handles,   const std::vector<std::string> & names,  
           const std::string & interface_type )
          {
           std::vector<T> result;
           for (  const auto & name : names )
           {
           std::copy_if(  
           unordered_handles.cbegin(   ),   unordered_handles.cend(   ),   std::back_inserter(  result ),  
           [&](  const auto & handle )
           {
           return (  handle.get_name(   ) == name ) && (  handle.get_interface_name(   ) == interface_type ) &&
           handle;
           } );
           }
           return result;
          }
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__ACCESSOR_HPP_

ros2_control/transmission_interface/include/transmission_interface/differential_transmission.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
          #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
          
          #include <cassert>
          #include <set>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "transmission_interface/accessor.hpp"
          #include "transmission_interface/exception.hpp"
          #include "transmission_interface/transmission.hpp"
          
          namespace transmission_interface
          {
          /// Implementation of a differential transmission.
          /**
           *
           * This transmission relates <b>two actuators</b> and <b>two joints</b> through a differential mechanism,   as illustrated
           * below.
           * \image html differential_transmission.png
           *
           * <CENTER>
           * <table>
           * <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
           * <tr><td>
           * <b> Actuator to joint </b>
           * </td>
           * <td>
           * \f{eqnarray*}{
           * \tau_{j_1} & = & n_{j_1} (   n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2}  ) \\[2.5em]
           * \tau_{j_2} & = & n_{j_2} (   n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2}  )
           * \f}
           * </td>
           * <td>
           * \f{eqnarray*}{
           * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} / n_{a_1} + \dot{x}_{a_2} / n_{a_2} }{2 n_{j_1}} \\[1em]
           * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_1} / n_{a_1} - \dot{x}_{a_2} / n_{a_2} }{2 n_{j_2}}
           * \f}
           * </td>
           * <td>
           * \f{eqnarray*}{
           * x_{j_1} & = & \frac{ x_{a_1} / n_{a_1} + x_{a_2} / n_{a_2} }{2 n_{j_1}} + x_{off_1} \\[1em]
           * x_{j_2} & = & \frac{ x_{a_1} / n_{a_1} - x_{a_2} / n_{a_2} }{2 n_{j_1}} + x_{off_2}
           * \f}
           * </td>
           * </tr>
           * <tr><td>
           * <b> Joint to actuator </b>
           * </td>
           * <td>
           * \f{eqnarray*}{
           * \tau_{a_1} & = & \frac{ \tau_{j_1} / n_{j_1} + \tau_{j_2} / n_{j_2} }{2 n_{a_1}} \\[1em]
           * \tau_{a_2} & = & \frac{ \tau_{j_1} / n_{j_1} - \tau_{j_2} / n_{j_2} }{2 n_{a_1}}
           * \f}
           * </td>
           * <td>
           * \f{eqnarray*}{
           * \dot{x}_{a_1} & = & n_{a_1} (   n_{j_1} \dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}  ) \\[2.5em]
           * \dot{x}_{a_2} & = & n_{a_2} (   n_{j_1} \dot{x}_{j_1} - n_{j_2} \dot{x}_{j_2}  )
           * \f}
           * </td>
           * <td>
           * \f{eqnarray*}{
           * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (  x_{j_1} - x_{off_1} ) + n_{j_2} (  x_{j_2} - x_{off_2} ) \right] \\[2.5em]
           * x_{a_2} & = & n_{a_2} \left[ n_{j_1} (  x_{j_1} - x_{off_1} ) - n_{j_2} (  x_{j_2} - x_{off_2} ) \right]
           * \f}
           * </td></tr></table>
           * </CENTER>
           *
           * where:
           * - \f$ x \f$,   \f$ \dot{x} \f$ and \f$ \tau \f$ are position,   velocity and effort variables,   respectively.
           * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables,   respectively.
           * - \f$ x_{off}\f$ represents the offset between motor and joint zeros,   expressed in joint position coordinates
           * (  cf. SimpleTransmission class documentation for a more detailed description of this variable ).
           * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator and joint sides
           * (  depicted as timing belts in the figure ).
           * A transmission ratio can take any real value \e except zero. In particular:
           * - If its absolute value is greater than one,   it's a velocity reducer / effort amplifier,   while if its absolute
           * value lies in \f$ (  0,   1 ) \f$ it's a velocity amplifier / effort reducer.
           * - Negative values represent a direction flip,   ie. input and output move in opposite directions.
           * - <b>Important:</b> Use transmission ratio signs to match this class' convention of positive actuator/joint
           * directions with a given mechanical design,   as they will in general not match.
           *
           * \note This implementation currently assumes a specific layout for location of the actuators and joint axes which is
           * common in robotic mechanisms. Please file an enhancement ticket if your use case does not adhere to this layout.
           *
           * \ingroup transmission_types
           */
     104  class DifferentialTransmission : public Transmission
          {
          public:
           /**
           * \param[in] actuator_reduction Reduction ratio of actuators.
           * \param[in] joint_reduction Reduction ratio of joints.
           * \param[in] joint_offset Joint position offset used in the position mappings.
           * \pre Nonzero actuator and joint reduction values.
           */
     113   DifferentialTransmission(  
           const std::vector<double> & actuator_reduction,   const std::vector<double> & joint_reduction,  
           const std::vector<double> & joint_offset = {0.0,   0.0} );
          
           /**
           * \param[in] joint_handles Handles of joint values.
           * \param[in] actuator_handles Handles of actuator values.
           * \pre Handles are valid and matching in size
           */
           void configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles ) override;
          
           /// Transform variables from actuator to joint space.
           /**
           * \pre Actuator and joint vectors must have size 2 and point to valid data.
           * To call this method it is not required that all other data vectors contain valid data,   and can even remain empty.
           */
           void actuator_to_joint(   ) override;
          
           /// Transform variables from joint to actuator space.
           /**
           * \pre Actuator and joint vectors must have size 2 and point to valid data.
           * To call this method it is not required that all other data vectors contain valid data,   and can even remain empty.
           */
           void joint_to_actuator(   ) override;
          
           std::size_t num_actuators(   ) const override { return 2; }
     141   std::size_t num_joints(   ) const override { return 2; }
          
     143   const std::vector<double> & get_actuator_reduction(   ) const { return actuator_reduction_; }
     144   const std::vector<double> & get_joint_reduction(   ) const { return joint_reduction_; }
     145   const std::vector<double> & get_joint_offset(   ) const { return joint_offset_; }
          
           /// Get human-friendly report of handles
     148   std::string get_handles_info(   ) const;
          
          protected:
           std::vector<double> actuator_reduction_;
           std::vector<double> joint_reduction_;
           std::vector<double> joint_offset_;
          
           std::vector<JointHandle> joint_position_;
           std::vector<JointHandle> joint_velocity_;
           std::vector<JointHandle> joint_effort_;
          
           std::vector<ActuatorHandle> actuator_position_;
           std::vector<ActuatorHandle> actuator_velocity_;
           std::vector<ActuatorHandle> actuator_effort_;
          };
          
          inline DifferentialTransmission::DifferentialTransmission(  
           const std::vector<double> & actuator_reduction,   const std::vector<double> & joint_reduction,  
           const std::vector<double> & joint_offset )
          : actuator_reduction_(  actuator_reduction ),  
           joint_reduction_(  joint_reduction ),  
           joint_offset_(  joint_offset )
          {
           if (  
           num_actuators(   ) != actuator_reduction_.size(   ) || num_joints(   ) != joint_reduction_.size(   ) ||
           num_joints(   ) != joint_offset_.size(   ) )
           {
           throw Exception(  "Reduction and offset vectors must have size 2." );
           }
          
           if (  
           0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
           0.0 == joint_reduction_[1] )
           {
           throw Exception(  "Transmission reduction ratios cannot be zero." );
           }
          }
          
          void DifferentialTransmission::configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles )
          {
           if (  joint_handles.empty(   ) )
           {
           throw Exception(  "No joint handles were passed in" );
           }
          
           if (  actuator_handles.empty(   ) )
           {
           throw Exception(  "No actuator handles were passed in" );
           }
          
           const auto joint_names = get_names(  joint_handles );
           if (  joint_names.size(   ) != 2 )
           {
           throw Exception(  
           "There should be exactly two unique joint names but was given " + to_string(  joint_names ) );
           }
           const auto actuator_names = get_names(  actuator_handles );
           if (  actuator_names.size(   ) != 2 )
           {
           throw Exception(  
           "There should be exactly two unique actuator names but was given " +
           to_string(  actuator_names ) );
           }
          
           joint_position_ =
           get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_POSITION );
           joint_velocity_ =
           get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_VELOCITY );
           joint_effort_ = get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_EFFORT );
          
           if (  joint_position_.size(   ) != 2 && joint_velocity_.size(   ) != 2 && joint_effort_.size(   ) != 2 )
           {
           throw Exception(  "Not enough valid or required joint handles were presented." );
           }
          
           actuator_position_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_POSITION );
           actuator_velocity_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_VELOCITY );
           actuator_effort_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_EFFORT );
          
           if (  
           actuator_position_.size(   ) != 2 && actuator_velocity_.size(   ) != 2 &&
           actuator_effort_.size(   ) != 2 )
           {
           throw Exception(  
           "Not enough valid or required actuator handles were presented. \n" + get_handles_info(   ) );
           }
          
           if (  
           joint_position_.size(   ) != actuator_position_.size(   ) &&
           joint_velocity_.size(   ) != actuator_velocity_.size(   ) &&
           joint_effort_.size(   ) != actuator_effort_.size(   ) )
           {
           throw Exception(  "Pair-wise mismatch on interfaces. \n" + get_handles_info(   ) );
           }
          }
          
          inline void DifferentialTransmission::actuator_to_joint(   )
          {
           const auto & ar = actuator_reduction_;
           const auto & jr = joint_reduction_;
          
           auto & act_pos = actuator_position_;
           auto & joint_pos = joint_position_;
           if (  act_pos.size(   ) == num_actuators(   ) && joint_pos.size(   ) == num_joints(   ) )
           {
           assert(  act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
          
           joint_pos[0].set_value(  
           (  act_pos[0].get_value(   ) / ar[0] + act_pos[1].get_value(   ) / ar[1] ) / (  2.0 * jr[0] ) +
           joint_offset_[0] );
           joint_pos[1].set_value(  
           (  act_pos[0].get_value(   ) / ar[0] - act_pos[1].get_value(   ) / ar[1] ) / (  2.0 * jr[1] ) +
           joint_offset_[1] );
           }
          
           auto & act_vel = actuator_velocity_;
           auto & joint_vel = joint_velocity_;
           if (  act_vel.size(   ) == num_actuators(   ) && joint_vel.size(   ) == num_joints(   ) )
           {
           assert(  act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
          
           joint_vel[0].set_value(  
           (  act_vel[0].get_value(   ) / ar[0] + act_vel[1].get_value(   ) / ar[1] ) / (  2.0 * jr[0] ) );
           joint_vel[1].set_value(  
           (  act_vel[0].get_value(   ) / ar[0] - act_vel[1].get_value(   ) / ar[1] ) / (  2.0 * jr[1] ) );
           }
          
           auto & act_eff = actuator_effort_;
           auto & joint_eff = joint_effort_;
           if (  act_eff.size(   ) == num_actuators(   ) && joint_eff.size(   ) == num_joints(   ) )
           {
           assert(  act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
          
           joint_eff[0].set_value(  
           jr[0] * (  act_eff[0].get_value(   ) * ar[0] + act_eff[1].get_value(   ) * ar[1] ) );
           joint_eff[1].set_value(  
           jr[1] * (  act_eff[0].get_value(   ) * ar[0] - act_eff[1].get_value(   ) * ar[1] ) );
           }
          }
          
          inline void DifferentialTransmission::joint_to_actuator(   )
          {
           const auto & ar = actuator_reduction_;
           const auto & jr = joint_reduction_;
          
           auto & act_pos = actuator_position_;
           auto & joint_pos = joint_position_;
           if (  act_pos.size(   ) == num_actuators(   ) && joint_pos.size(   ) == num_joints(   ) )
           {
           assert(  act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
          
           double joints_offset_applied[2] = {
           joint_pos[0].get_value(   ) - joint_offset_[0],   joint_pos[1].get_value(   ) - joint_offset_[1]};
           act_pos[0].set_value(  
           (  joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1] ) * ar[0] );
           act_pos[1].set_value(  
           (  joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1] ) * ar[1] );
           }
          
           auto & act_vel = actuator_velocity_;
           auto & joint_vel = joint_velocity_;
           if (  act_vel.size(   ) == num_actuators(   ) && joint_vel.size(   ) == num_joints(   ) )
           {
           assert(  act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
          
           act_vel[0].set_value(  
           (  joint_vel[0].get_value(   ) * jr[0] + joint_vel[1].get_value(   ) * jr[1] ) * ar[0] );
           act_vel[1].set_value(  
           (  joint_vel[0].get_value(   ) * jr[0] - joint_vel[1].get_value(   ) * jr[1] ) * ar[1] );
           }
          
           auto & act_eff = actuator_effort_;
           auto & joint_eff = joint_effort_;
           if (  act_eff.size(   ) == num_actuators(   ) && joint_eff.size(   ) == num_joints(   ) )
           {
           assert(  act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
          
           act_eff[0].set_value(  
           (  joint_eff[0].get_value(   ) / jr[0] + joint_eff[1].get_value(   ) / jr[1] ) / (  2.0 * ar[0] ) );
           act_eff[1].set_value(  
           (  joint_eff[0].get_value(   ) / jr[0] - joint_eff[1].get_value(   ) / jr[1] ) / (  2.0 * ar[1] ) );
           }
          }
          
          std::string DifferentialTransmission::get_handles_info(   ) const
          {
           return std::string(  "Got the following handles:\n" ) +
           "Joint position: " + to_string(  get_names(  joint_position_ ) ) +
           ",   Actuator position: " + to_string(  get_names(  actuator_position_ ) ) + "\n" +
           "Joint velocity: " + to_string(  get_names(  joint_velocity_ ) ) +
           ",   Actuator velocity: " + to_string(  get_names(  actuator_velocity_ ) ) + "\n" +
           "Joint effort: " + to_string(  get_names(  joint_effort_ ) ) +
           ",   Actuator effort: " + to_string(  get_names(  actuator_effort_ ) );
          }
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_

ros2_control/transmission_interface/include/transmission_interface/differential_transmission_loader.hpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_
          #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_
          
          #include <memory>
          
          #include "transmission_interface/transmission.hpp"
          #include "transmission_interface/transmission_loader.hpp"
          
          namespace transmission_interface
          {
          /**
           * \brief Class for loading a four-bar linkage transmission instance from configuration data.
           */
      28  class DifferentialTransmissionLoader : public TransmissionLoader
          {
          public:
           std::shared_ptr<Transmission> load(  
           const hardware_interface::TransmissionInfo & transmission_info ) override;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_

ros2_control/transmission_interface/include/transmission_interface/exception.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          #ifndef TRANSMISSION_INTERFACE__EXCEPTION_HPP_
          #define TRANSMISSION_INTERFACE__EXCEPTION_HPP_
          
          #include <exception>
          #include <string>
          
          namespace transmission_interface
          {
      22  class Exception : public std::exception
          {
          public:
      25   explicit Exception(  const char * message ) : msg(  message ) {}
      26   explicit Exception(  const std::string & message ) : msg(  message ) {}
           const char * what(   ) const noexcept override { return msg.c_str(   ); }
          
          private:
           std::string msg;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__EXCEPTION_HPP_

ros2_control/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_
          #define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_
          
          #include <cassert>
          #include <string>
          #include <vector>
          
          #include "transmission_interface/accessor.hpp"
          #include "transmission_interface/exception.hpp"
          #include "transmission_interface/transmission.hpp"
          
          namespace transmission_interface
          {
          /// Implementation of a four-bar-linkage transmission.
          /**
          *
          * This transmission relates <b>two actuators</b> and <b>two joints</b> through a mechanism in which the state of the
          * first joint only depends on the first actuator,   while the second joint depends on both actuators,   as
          * illustrated below.
          * Although the class name makes specific reference to the four-bar-linkage,   there are other mechanical layouts
          * that yield the same behavior,   such as the remote actuation example also depicted below.
          * \image html four_bar_linkage_transmission.png
          *
          * <CENTER>
          * <table>
          * <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
          * <tr><td>
          * <b> Actuator to joint </b>
          * </td>
          * <td>
          * \f{eqnarray*}{
          * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\
          * \tau_{j_2} & = & n_{j_2} (  n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1} )
          * \f}
          * </td>
          * <td>
          * \f{eqnarray*}{
          * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\
          * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (  n_{j_1} n_{a_1} ) }{ n_{j_2} }
          * \f}
          * </td>
          * <td>
          * \f{eqnarray*}{
          * x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\
          * x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (  n_{j_1} n_{a_1} ) }{ n_{j_2} } + x_{off_2}
          * \f}
          * </td>
          * </tr>
          * <tr><td>
          * <b> Joint to actuator </b>
          * </td>
          * <td>
          * \f{eqnarray*}{
          * \tau_{a_1} & = & \tau_{j_1} / (  n_{j_1} n_{a_1} ) \\
          * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} }
          * \f}
          * </td>
          * <td>
          * \f{eqnarray*}{
          * \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\
          * \dot{x}_{a_2} & = & n_{a_2} (  \dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2} )
          * \f}
          * </td>
          * <td>
          * \f{eqnarray*}{
          * x_{a_1} & = & n_{j_1} n_{a_1} (  x_{j_1} - x_{off_1} ) \\
          * x_{a_2} & = & n_{a_2} \left[(  x_{j_1} - x_{off_1} ) + n_{j_2} (  x_{j_2} - x_{off_2} )\right]
          * \f}
          * </td></tr></table>
          * </CENTER>
          *
          * where:
          * - \f$ x \f$,   \f$ \dot{x} \f$ and \f$ \tau \f$ are position,   velocity and effort variables,   respectively.
          * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables,   respectively.
          * - \f$ x_{off}\f$ represents the offset between motor and joint zeros,   expressed in joint position coordinates.
          * (  cf. SimpleTransmission class documentation for a more detailed description of this variable ).
          * - \f$ n \f$ represents a transmission ratio (  reducers/amplifiers are depicted as timing belts in the figure ).
          * A transmission ratio can take any real value \e except zero. In particular:
          * - If its absolute value is greater than one,   it's a velocity reducer / effort amplifier,   while if its absolute
          * value lies in \f$ (  0,   1 ) \f$ it's a velocity amplifier / effort reducer.
          * - Negative values represent a direction flip,   ie. input and output move in opposite directions.
          * - <b>Important:</b> Use transmission ratio signs to match this class' convention of positive actuator/joint
          * directions with a given mechanical design,   as they will in general not match.
          *
          * \ingroup transmission_types
          */
     103  class FourBarLinkageTransmission : public Transmission
          {
          public:
           /**
           * \param actuator_reduction Reduction ratio of actuators.
           * \param joint_reduction Reduction ratio of joints.
           * \param joint_offset Joint position offset used in the position mappings.
           * \pre Nonzero actuator reduction values.
           */
     112   FourBarLinkageTransmission(  
           const std::vector<double> & actuator_reduction,   const std::vector<double> & joint_reduction,  
           const std::vector<double> & joint_offset = std::vector<double>(  2,   0.0 ) );
          
           /**
           * \param[in] joint_handles Handles of joint values.
           * \param[in] actuator_handles Handles of actuator values.
           * \pre Handles are valid and matching in size
           */
           void configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles ) override;
          
           /// Transform variables from actuator to joint space.
           /**
           * \pre Actuator and joint vectors must have size 2 and point to valid data.
           * To call this method it is not required that all other data vectors contain valid data,   and can even remain empty.
           */
           void actuator_to_joint(   ) override;
          
           /// Transform variables from joint to actuator space.
           /**
           * \pre Actuator and joint vectors must have size 2 and point to valid data.
           * To call this method it is not required that all other data vectors contain valid data,   and can even remain empty.
           */
           void joint_to_actuator(   ) override;
          
           std::size_t num_actuators(   ) const override { return 2; }
     140   std::size_t num_joints(   ) const override { return 2; }
          
     142   const std::vector<double> & get_actuator_reduction(   ) const { return actuator_reduction_; }
     143   const std::vector<double> & get_joint_reduction(   ) const { return joint_reduction_; }
     144   const std::vector<double> & get_joint_offset(   ) const { return joint_offset_; }
          
           /// Get human-friendly report of handles
     147   std::string get_handles_info(   ) const;
          
          protected:
           std::vector<double> actuator_reduction_;
           std::vector<double> joint_reduction_;
           std::vector<double> joint_offset_;
          
           std::vector<JointHandle> joint_position_;
           std::vector<JointHandle> joint_velocity_;
           std::vector<JointHandle> joint_effort_;
          
           std::vector<ActuatorHandle> actuator_position_;
           std::vector<ActuatorHandle> actuator_velocity_;
           std::vector<ActuatorHandle> actuator_effort_;
          };
          
          inline FourBarLinkageTransmission::FourBarLinkageTransmission(  
           const std::vector<double> & actuator_reduction,   const std::vector<double> & joint_reduction,  
           const std::vector<double> & joint_offset )
          : actuator_reduction_(  actuator_reduction ),  
           joint_reduction_(  joint_reduction ),  
           joint_offset_(  joint_offset )
          {
           if (  
           num_actuators(   ) != actuator_reduction_.size(   ) || num_joints(   ) != joint_reduction_.size(   ) ||
           num_joints(   ) != joint_offset_.size(   ) )
           {
           throw Exception(  "Reduction and offset vectors must have size 2." );
           }
           if (  
           0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
           0.0 == joint_reduction_[1] )
           {
           throw Exception(  "Transmission reduction ratios cannot be zero." );
           }
          }
          
          void FourBarLinkageTransmission::configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles )
          {
           if (  joint_handles.empty(   ) )
           {
           throw Exception(  "No joint handles were passed in" );
           }
          
           if (  actuator_handles.empty(   ) )
           {
           throw Exception(  "No actuator handles were passed in" );
           }
          
           const auto joint_names = get_names(  joint_handles );
           if (  joint_names.size(   ) != 2 )
           {
           throw Exception(  
           "There should be exactly two unique joint names but was given " + to_string(  joint_names ) );
           }
           const auto actuator_names = get_names(  actuator_handles );
           if (  actuator_names.size(   ) != 2 )
           {
           throw Exception(  
           "There should be exactly two unique actuator names but was given " +
           to_string(  actuator_names ) );
           }
          
           joint_position_ =
           get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_POSITION );
           joint_velocity_ =
           get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_VELOCITY );
           joint_effort_ = get_ordered_handles(  joint_handles,   joint_names,   hardware_interface::HW_IF_EFFORT );
          
           if (  joint_position_.size(   ) != 2 && joint_velocity_.size(   ) != 2 && joint_effort_.size(   ) != 2 )
           {
           throw Exception(  "Not enough valid or required joint handles were presented." );
           }
          
           actuator_position_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_POSITION );
           actuator_velocity_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_VELOCITY );
           actuator_effort_ =
           get_ordered_handles(  actuator_handles,   actuator_names,   hardware_interface::HW_IF_EFFORT );
          
           if (  
           actuator_position_.size(   ) != 2 && actuator_velocity_.size(   ) != 2 &&
           actuator_effort_.size(   ) != 2 )
           {
           throw Exception(  
           "Not enough valid or required actuator handles were presented. \n" + get_handles_info(   ) );
           }
          
           if (  
           joint_position_.size(   ) != actuator_position_.size(   ) &&
           joint_velocity_.size(   ) != actuator_velocity_.size(   ) &&
           joint_effort_.size(   ) != actuator_effort_.size(   ) )
           {
           throw Exception(  "Pair-wise mismatch on interfaces. \n" + get_handles_info(   ) );
           }
          }
          
          inline void FourBarLinkageTransmission::actuator_to_joint(   )
          {
           const auto & ar = actuator_reduction_;
           const auto & jr = joint_reduction_;
          
           // position
           auto & act_pos = actuator_position_;
           auto & joint_pos = joint_position_;
           if (  act_pos.size(   ) == num_actuators(   ) && joint_pos.size(   ) == num_joints(   ) )
           {
           assert(  act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
          
           joint_pos[0].set_value(  act_pos[0].get_value(   ) / (  jr[0] * ar[0] ) + joint_offset_[0] );
           joint_pos[1].set_value(  
           (  act_pos[1].get_value(   ) / ar[1] - act_pos[0].get_value(   ) / (  jr[0] * ar[0] ) ) / jr[1] +
           joint_offset_[1] );
           }
          
           // velocity
           auto & act_vel = actuator_velocity_;
           auto & joint_vel = joint_velocity_;
           if (  act_vel.size(   ) == num_actuators(   ) && joint_vel.size(   ) == num_joints(   ) )
           {
           assert(  act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
          
           joint_vel[0].set_value(  act_vel[0].get_value(   ) / (  jr[0] * ar[0] ) );
           joint_vel[1].set_value(  
           (  act_vel[1].get_value(   ) / ar[1] - act_vel[0].get_value(   ) / (  jr[0] * ar[0] ) ) / jr[1] );
           }
          
           // effort
           auto & act_eff = actuator_effort_;
           auto & joint_eff = joint_effort_;
           if (  act_eff.size(   ) == num_actuators(   ) && joint_eff.size(   ) == num_joints(   ) )
           {
           assert(  act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
          
           joint_eff[0].set_value(  jr[0] * act_eff[0].get_value(   ) * ar[0] );
           joint_eff[1].set_value(  
           jr[1] * (  act_eff[1].get_value(   ) * ar[1] - act_eff[0].get_value(   ) * ar[0] * jr[0] ) );
           }
          }
          
          inline void FourBarLinkageTransmission::joint_to_actuator(   )
          {
           const auto & ar = actuator_reduction_;
           const auto & jr = joint_reduction_;
          
           // position
           auto & act_pos = actuator_position_;
           auto & joint_pos = joint_position_;
           if (  act_pos.size(   ) == num_actuators(   ) && joint_pos.size(   ) == num_joints(   ) )
           {
           assert(  act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
          
           double joints_offset_applied[2] = {
           joint_pos[0].get_value(   ) - joint_offset_[0],   joint_pos[1].get_value(   ) - joint_offset_[1]};
           act_pos[0].set_value(  joints_offset_applied[0] * jr[0] * ar[0] );
           act_pos[1].set_value(  (  joints_offset_applied[0] + joints_offset_applied[1] * jr[1] ) * ar[1] );
           }
          
           // velocity
           auto & act_vel = actuator_velocity_;
           auto & joint_vel = joint_velocity_;
           if (  act_vel.size(   ) == num_actuators(   ) && joint_vel.size(   ) == num_joints(   ) )
           {
           assert(  act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
          
           act_vel[0].set_value(  joint_vel[0].get_value(   ) * jr[0] * ar[0] );
           act_vel[1].set_value(  (  joint_vel[0].get_value(   ) + joint_vel[1].get_value(   ) * jr[1] ) * ar[1] );
           }
          
           // effort
           auto & act_eff = actuator_effort_;
           auto & joint_eff = joint_effort_;
           if (  act_eff.size(   ) == num_actuators(   ) && joint_eff.size(   ) == num_joints(   ) )
           {
           assert(  act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
          
           act_eff[0].set_value(  joint_eff[0].get_value(   ) / (  ar[0] * jr[0] ) );
           act_eff[1].set_value(  (  joint_eff[0].get_value(   ) + joint_eff[1].get_value(   ) / jr[1] ) / ar[1] );
           }
          }
          
          std::string FourBarLinkageTransmission::get_handles_info(   ) const
          {
           return std::string(  "Got the following handles:\n" ) +
           "Joint position: " + to_string(  get_names(  joint_position_ ) ) +
           ",   Actuator position: " + to_string(  get_names(  actuator_position_ ) ) + "\n" +
           "Joint velocity: " + to_string(  get_names(  joint_velocity_ ) ) +
           ",   Actuator velocity: " + to_string(  get_names(  actuator_velocity_ ) ) + "\n" +
           "Joint effort: " + to_string(  get_names(  joint_effort_ ) ) +
           ",   Actuator effort: " + to_string(  get_names(  actuator_effort_ ) );
          }
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_

ros2_control/transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
          #define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
          
          #include <memory>
          
          #include "transmission_interface/transmission.hpp"
          #include "transmission_interface/transmission_loader.hpp"
          
          namespace transmission_interface
          {
          /**
           * \brief Class for loading a four-bar linkage transmission instance from configuration data.
           */
      28  class FourBarLinkageTransmissionLoader : public TransmissionLoader
          {
          public:
           std::shared_ptr<Transmission> load(  
           const hardware_interface::TransmissionInfo & transmission_info ) override;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_

ros2_control/transmission_interface/include/transmission_interface/handle.hpp

       1  // Copyright 2020 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_
          #define TRANSMISSION_INTERFACE__HANDLE_HPP_
          
          #include <string>
          
          #include "hardware_interface/handle.hpp"
          
          namespace transmission_interface
          {
          /** A handle used to get and set a value on a given actuator interface. */
      25  class ActuatorHandle : public hardware_interface::ReadWriteHandle
          {
          public:
           using hardware_interface::ReadWriteHandle::ReadWriteHandle;
          };
          
          /** A handle used to get and set a value on a given joint interface. */
      32  class JointHandle : public hardware_interface::ReadWriteHandle
          {
          public:
           using hardware_interface::ReadWriteHandle::ReadWriteHandle;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__HANDLE_HPP_

ros2_control/transmission_interface/include/transmission_interface/simple_transmission.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
          #define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
          
          #include <algorithm>
          #include <cassert>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "transmission_interface/exception.hpp"
          #include "transmission_interface/transmission.hpp"
          
          namespace transmission_interface
          {
          /// Implementation of a simple reducer transmission.
          /**
           * This transmission relates <b>one actuator</b> and <b>one joint</b> through a reductor (  or amplifier ).
           * Timing belts and gears are examples of this transmission type,   and are illustrated below.
           * \image html simple_transmission.png
           *
           * <CENTER>
           * <table>
           * <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
           * <tr><td>
           * <b> Actuator to joint </b>
           * </td>
           * <td>
           * \f[ \tau_j = n \tau_a \f]
           * </td>
           * <td>
           * \f[ \dot{x}_j = \dot{x}_a / n \f]
           * </td>
           * <td>
           * \f[ x_j = x_a / n + x_{off} \f]
           * </td>
           * </tr>
           * <tr><td>
           * <b> Joint to actuator </b>
           * </td>
           * <td>
           * \f[ \tau_a = \tau_j / n\f]
           * </td>
           * <td>
           * \f[ \dot{x}_a = n \dot{x}_j \f]
           * </td>
           * <td>
           * \f[ x_a = n (  x_j - x_{off} ) \f]
           * </td></tr></table>
           * </CENTER>
           *
           * where:
           * - \f$ x \f$,   \f$ \dot{x} \f$ and \f$ \tau \f$ are position,   velocity and effort variables,   respectively.
           * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables,   respectively.
           * - \f$ x_{off}\f$ represents the offset between motor and joint zeros,   expressed in joint position coordinates.
           * - \f$ n \f$ is the transmission ratio,   and can be computed as the ratio between the output and input pulley
           * radii for the timing belt; or the ratio between output and input teeth for the gear system.
           * The transmission ratio can take any real value \e except zero. In particular:
           * - If its absolute value is greater than one,   it's a velocity reducer / effort amplifier,   while if its absolute
           * value lies in \f$ (  0,   1 ) \f$ it's a velocity amplifier / effort reducer.
           * - Negative values represent a direction flip,   ie. actuator and joint move in opposite directions. For example,  
           * in timing belts actuator and joint move in the same direction,   while in single-stage gear systems actuator and
           * joint move in opposite directions.
           *
           * \ingroup transmission_types
           */
      80  class SimpleTransmission : public Transmission
          {
          public:
           /**
           * \param[in] joint_to_actuator_reduction Joint to actuator reduction ratio.
           * \param[in] joint_offset Joint position offset used in the position mappings.
           * \pre Nonzero reduction value.
           */
      88   explicit SimpleTransmission(  
           const double joint_to_actuator_reduction,   const double joint_offset = 0.0 );
          
           /// Set up the data the transmission operates on.
           /**
           * \param[in] joint_handles Vector of interface handles of one joint
           * \param[in] actuator_handles Vector of interface handles of one actuator
           * \pre Vectors are nonzero.
           * \pre Joint handles share the same joint name and actuator handles share the same actuator name.
           */
           void configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles ) override;
          
           /// Transform variables from actuator to joint space.
           /**
           * This method operates on the handles provided when configuring the transmission.
           * To call this method it is not required that all supported interface types are provided,   e.g. one can supply only velocity handles
           */
           void actuator_to_joint(   ) override;
          
           /// Transform variables from joint to actuator space.
           /**
           * This method operates on the handles provided when configuring the transmission.
           * To call this method it is not required that all supported interface types are provided,   e.g. one can supply only velocity handles
           */
           void joint_to_actuator(   ) override;
          
           std::size_t num_actuators(   ) const override { return 1; }
     117   std::size_t num_joints(   ) const override { return 1; }
          
     119   double get_actuator_reduction(   ) const { return reduction_; }
     120   double get_joint_offset(   ) const { return jnt_offset_; }
          
          protected:
           double reduction_;
           double jnt_offset_;
          
           JointHandle joint_position_ = {"",   "",   nullptr};
           JointHandle joint_velocity_ = {"",   "",   nullptr};
           JointHandle joint_effort_ = {"",   "",   nullptr};
          
           ActuatorHandle actuator_position_ = {"",   "",   nullptr};
           ActuatorHandle actuator_velocity_ = {"",   "",   nullptr};
           ActuatorHandle actuator_effort_ = {"",   "",   nullptr};
          };
          
          inline SimpleTransmission::SimpleTransmission(  
           const double joint_to_actuator_reduction,   const double joint_offset )
          : reduction_(  joint_to_actuator_reduction ),   jnt_offset_(  joint_offset )
          {
           if (  reduction_ == 0.0 )
           {
           throw Exception(  "Transmission reduction ratio cannot be zero." );
           }
          }
          
          template <class HandleType>
          HandleType get_by_interface(  
           const std::vector<HandleType> & handles,   const std::string & interface_name )
          {
           const auto result = std::find_if(  
           handles.cbegin(   ),   handles.cend(   ),  
           [&interface_name](  const auto handle ) { return handle.get_interface_name(   ) == interface_name; } );
           if (  result == handles.cend(   ) )
           {
           return HandleType(  handles.cbegin(   )->get_prefix_name(   ),   interface_name,   nullptr );
           }
           return *result;
          }
          
          template <class T>
          bool are_names_identical(  const std::vector<T> & handles )
          {
           std::vector<std::string> names;
           std::transform(  
           handles.cbegin(   ),   handles.cend(   ),   std::back_inserter(  names ),  
           [](  const auto & handle ) { return handle.get_prefix_name(   ); } );
           return std::equal(  names.cbegin(   ) + 1,   names.cend(   ),   names.cbegin(   ) );
          }
          
          inline void SimpleTransmission::configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles )
          {
           if (  joint_handles.empty(   ) )
           {
           throw Exception(  "No joint handles were passed in" );
           }
          
           if (  actuator_handles.empty(   ) )
           {
           throw Exception(  "No actuator handles were passed in" );
           }
          
           if (  !are_names_identical(  joint_handles ) )
           {
           throw Exception(  "Joint names given to transmissions should be identical" );
           }
          
           if (  !are_names_identical(  actuator_handles ) )
           {
           throw Exception(  "Actuator names given to transmissions should be identical" );
           }
          
           joint_position_ = get_by_interface(  joint_handles,   hardware_interface::HW_IF_POSITION );
           joint_velocity_ = get_by_interface(  joint_handles,   hardware_interface::HW_IF_VELOCITY );
           joint_effort_ = get_by_interface(  joint_handles,   hardware_interface::HW_IF_EFFORT );
          
           if (  !joint_position_ && !joint_velocity_ && !joint_effort_ )
           {
           throw Exception(  "None of the provided joint handles are valid or from the required interfaces" );
           }
          
           actuator_position_ = get_by_interface(  actuator_handles,   hardware_interface::HW_IF_POSITION );
           actuator_velocity_ = get_by_interface(  actuator_handles,   hardware_interface::HW_IF_VELOCITY );
           actuator_effort_ = get_by_interface(  actuator_handles,   hardware_interface::HW_IF_EFFORT );
          
           if (  !actuator_position_ && !actuator_velocity_ && !actuator_effort_ )
           {
           throw Exception(  "None of the provided joint handles are valid or from the required interfaces" );
           }
          }
          
          inline void SimpleTransmission::actuator_to_joint(   )
          {
           if (  joint_effort_ && actuator_effort_ )
           {
           joint_effort_.set_value(  actuator_effort_.get_value(   ) * reduction_ );
           }
          
           if (  joint_velocity_ && actuator_velocity_ )
           {
           joint_velocity_.set_value(  actuator_velocity_.get_value(   ) / reduction_ );
           }
          
           if (  joint_position_ && actuator_position_ )
           {
           joint_position_.set_value(  actuator_position_.get_value(   ) / reduction_ + jnt_offset_ );
           }
          }
          
          inline void SimpleTransmission::joint_to_actuator(   )
          {
           if (  joint_effort_ && actuator_effort_ )
           {
           actuator_effort_.set_value(  joint_effort_.get_value(   ) / reduction_ );
           }
          
           if (  joint_velocity_ && actuator_velocity_ )
           {
           actuator_velocity_.set_value(  joint_velocity_.get_value(   ) * reduction_ );
           }
          
           if (  joint_position_ && actuator_position_ )
           {
           actuator_position_.set_value(  (  joint_position_.get_value(   ) - jnt_offset_ ) * reduction_ );
           }
          }
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_

ros2_control/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_
          #define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_
          
          #include <memory>
          
          #include <transmission_interface/transmission.hpp>
          #include <transmission_interface/transmission_loader.hpp>
          
          namespace transmission_interface
          {
          /**
           * \brief Class for loading a simple transmission instance from configuration data.
           */
      28  class SimpleTransmissionLoader : public TransmissionLoader
          {
          public:
           std::shared_ptr<Transmission> load(  
           const hardware_interface::TransmissionInfo & transmission_info ) override;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_

ros2_control/transmission_interface/include/transmission_interface/transmission.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_HPP_
          #define TRANSMISSION_INTERFACE__TRANSMISSION_HPP_
          
          #include <cstddef>
          #include <memory>
          #include <stdexcept>
          #include <string>
          #include <vector>
          
          #include "transmission_interface/handle.hpp"
          
          namespace transmission_interface
          {
          /// Abstract base class for representing mechanical transmissions.
          /**
           * Mechanical transmissions transform effort/flow variables such that their product (  power ) remains constant.
           * Effort variables for linear and rotational domains are \e force and \e torque; while the flow variables are
           * respectively linear velocity and angular velocity.
           *
           * In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this
           * naming to identify the input and output spaces of the transformation.
           * The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort,   velocity and
           * position. Position is not a power variable,   but the mappings can be implemented using the velocity map plus an
           * integration constant representing the offset between actuator and joint zeros.
           *
           * \par Credit
           * This interface was inspired by similar existing implementations by PAL Robotics,   S.L. and Willow Garage Inc.
           *
           * \note Implementations of this interface must take care of realtime-safety if the code is to be run in realtime
           * contexts,   as is often the case in robot control.
           */
      45  class Transmission
          {
          public:
           virtual ~Transmission(   ) = default;
          
           virtual void configure(  
           const std::vector<JointHandle> & joint_handles,  
           const std::vector<ActuatorHandle> & actuator_handles ) = 0;
          
           /// Transform \e effort variables from actuator to joint space.
           /**
           * \param[in] act_data Actuator-space variables.
           * \param[out] jnt_data Joint-space variables.
           * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of
           * transmission actuators and joints.
           * Data vectors not used in this map can remain empty.
           */
           virtual void actuator_to_joint(   ) = 0;
          
           /// Transform \e effort variables from joint to actuator space.
           /**
           * \param[in] jnt_data Joint-space variables.
           * \param[out] act_data Actuator-space variables.
           * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of
           * transmission actuators and joints.
           * Data vectors not used in this map can remain empty.
           */
           virtual void joint_to_actuator(   ) = 0;
          
           /** \return Number of actuators managed by transmission,  
           * ie. the dimension of the actuator space.
           */
           virtual std::size_t num_actuators(   ) const = 0;
          
           /** \return Number of joints managed by transmission,  
           * ie. the dimension of the joint space.
           */
           virtual std::size_t num_joints(   ) const = 0;
          };
          
          typedef std::shared_ptr<Transmission> TransmissionSharedPtr;
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__TRANSMISSION_HPP_

ros2_control/transmission_interface/include/transmission_interface/transmission_interface_exception.hpp

          // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_
          #define TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_
          
          #include <exception>
          #include <string>
          
          namespace transmission_interface
          {
      23  class TransmissionInterfaceException : public std::exception
          {
          public:
      26   explicit TransmissionInterfaceException(  const std::string & message ) : msg(  message ) {}
           const char * what(   ) const noexcept override { return msg.c_str(   ); }
          
          private:
           std::string msg;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_

ros2_control/transmission_interface/include/transmission_interface/transmission_loader.hpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_
          #define TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_
          
          #include <memory>
          
          #include "hardware_interface/hardware_info.hpp"
          
          namespace transmission_interface
          {
          /**
           * \brief Abstract interface for loading transmission instances from configuration data.
           *
           * It also provides convenience methods for specific transmission loaders to leverage.
           */
      29  class TransmissionLoader
          {
          public:
           virtual ~TransmissionLoader(   ) = default;
          
           virtual std::shared_ptr<Transmission> load(  
           const hardware_interface::TransmissionInfo & transmission_info ) = 0;
          };
          
          } // namespace transmission_interface
          
          #endif // TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_

ros2_control/transmission_interface/include/transmission_interface/visibility_control.h

       1  // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_
          #define TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define TRANSMISSION_INTERFACE_EXPORT __attribute__(  (  dllexport ) )
          #define TRANSMISSION_INTERFACE_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define TRANSMISSION_INTERFACE_EXPORT __declspec(  dllexport )
          #define TRANSMISSION_INTERFACE_IMPORT __declspec(  dllimport )
          #endif
          #ifdef TRANSMISSION_INTERFACE_BUILDING_DLL
          #define TRANSMISSION_INTERFACE_PUBLIC TRANSMISSION_INTERFACE_EXPORT
          #else
          #define TRANSMISSION_INTERFACE_PUBLIC TRANSMISSION_INTERFACE_IMPORT
          #endif
          #define TRANSMISSION_INTERFACE_PUBLIC_TYPE TRANSMISSION_INTERFACE_PUBLIC
          #define TRANSMISSION_INTERFACE_LOCAL
          #else
          #define TRANSMISSION_INTERFACE_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define TRANSMISSION_INTERFACE_IMPORT
          #if __GNUC__ >= 4
          #define TRANSMISSION_INTERFACE_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define TRANSMISSION_INTERFACE_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define TRANSMISSION_INTERFACE_PUBLIC
          #define TRANSMISSION_INTERFACE_LOCAL
          #endif
          #define TRANSMISSION_INTERFACE_PUBLIC_TYPE
          #endif
          
          #endif // TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_

ros2_control/transmission_interface/src/differential_transmission_loader.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "transmission_interface/differential_transmission_loader.hpp"
          
          #include <memory>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "transmission_interface/differential_transmission.hpp"
          
          namespace transmission_interface
          {
      29  std::shared_ptr<Transmission> DifferentialTransmissionLoader::load(  
           const hardware_interface::TransmissionInfo & transmission_info )
          {
           try
           {
           const auto act_reduction1 = transmission_info.actuators.at(  0 ).mechanical_reduction;
           const auto act_reduction2 = transmission_info.actuators.at(  1 ).mechanical_reduction;
          
           const auto jnt_reduction1 = transmission_info.joints.at(  0 ).mechanical_reduction;
           const auto jnt_reduction2 = transmission_info.joints.at(  1 ).mechanical_reduction;
          
           const auto jnt_offset1 = transmission_info.joints.at(  0 ).offset;
           const auto jnt_offset2 = transmission_info.joints.at(  1 ).offset;
          
           std::shared_ptr<Transmission> transmission(  new DifferentialTransmission(  
           {act_reduction1,   act_reduction2},   {jnt_reduction1,   jnt_reduction2},  
           {jnt_offset1,   jnt_offset2} ) );
           return transmission;
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "differential_transmission_loader" ),  
           "Failed to construct transmission '%s'",   ex.what(   ) );
           return std::shared_ptr<Transmission>(   );
           }
          }
          
          } // namespace transmission_interface
          
      59  PLUGINLIB_EXPORT_CLASS(  
           transmission_interface::DifferentialTransmissionLoader,  
           transmission_interface::TransmissionLoader )

ros2_control/transmission_interface/src/four_bar_linkage_transmission_loader.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "transmission_interface/four_bar_linkage_transmission_loader.hpp"
          
          #include <memory>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "transmission_interface/four_bar_linkage_transmission.hpp"
          
          namespace transmission_interface
          {
      29  std::shared_ptr<Transmission> FourBarLinkageTransmissionLoader::load(  
           const hardware_interface::TransmissionInfo & transmission_info )
          {
           try
           {
           const auto act_reduction1 = transmission_info.actuators.at(  0 ).mechanical_reduction;
           const auto act_reduction2 = transmission_info.actuators.at(  1 ).mechanical_reduction;
          
           const auto jnt_reduction1 = transmission_info.joints.at(  0 ).mechanical_reduction;
           const auto jnt_reduction2 = transmission_info.joints.at(  1 ).mechanical_reduction;
          
           const auto jnt_offset1 = transmission_info.joints.at(  0 ).offset;
           const auto jnt_offset2 = transmission_info.joints.at(  1 ).offset;
          
           std::shared_ptr<Transmission> transmission(  new FourBarLinkageTransmission(  
           {act_reduction1,   act_reduction2},   {jnt_reduction1,   jnt_reduction2},  
           {jnt_offset1,   jnt_offset2} ) );
           return transmission;
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "four_bar_linkage_transmission_loader" ),  
           "Failed to construct transmission '%s'",   ex.what(   ) );
           return std::shared_ptr<Transmission>(   );
           }
          }
          
          } // namespace transmission_interface
          
      59  PLUGINLIB_EXPORT_CLASS(  
           transmission_interface::FourBarLinkageTransmissionLoader,  
           transmission_interface::TransmissionLoader )

ros2_control/transmission_interface/src/simple_transmission_loader.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "transmission_interface/simple_transmission_loader.hpp"
          
          #include <memory>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_list_macros.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "transmission_interface/simple_transmission.hpp"
          
          namespace transmission_interface
          {
      29  std::shared_ptr<Transmission> SimpleTransmissionLoader::load(  
           const hardware_interface::TransmissionInfo & transmission_info )
          {
           try
           {
           const auto mechanical_reduction = transmission_info.joints.at(  0 ).mechanical_reduction;
           const auto offset = transmission_info.joints.at(  0 ).offset;
           std::shared_ptr<Transmission> transmission(  
           new SimpleTransmission(  mechanical_reduction,   offset ) );
           return transmission;
           }
           catch (  const std::exception & ex )
           {
           RCLCPP_ERROR(  
           rclcpp::get_logger(  "simple_transmission_loader" ),   "Failed to construct transmission '%s'",  
           ex.what(   ) );
           return std::shared_ptr<Transmission>(   );
           }
          }
          
          } // namespace transmission_interface
          
      51  PLUGINLIB_EXPORT_CLASS(  
           transmission_interface::SimpleTransmissionLoader,   transmission_interface::TransmissionLoader )

ros2_control/transmission_interface/test/differential_transmission_loader_test.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <exception>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "transmission_interface/differential_transmission.hpp"
          #include "transmission_interface/differential_transmission_loader.hpp"
          #include "transmission_interface/transmission_loader.hpp"
          
          using testing::SizeIs;
          
      31  class TransmissionPluginLoader
          {
          public:
      34   std::shared_ptr<transmission_interface::TransmissionLoader> create(  const std::string & type )
           {
           try
           {
           return class_loader_.createUniqueInstance(  type );
           }
           catch (  std::exception & ex )
           {
           std::cerr << ex.what(   ) << std::endl;
           return std::shared_ptr<transmission_interface::TransmissionLoader>(   );
           }
           }
          
          private:
           // must keep it alive because instance destroyers need it
      49   pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
           "transmission_interface",   "transmission_interface::TransmissionLoader"};
          };
          
      53  TEST(  DifferentialTransmissionLoaderTest,   FullSpec )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>0.5</offset>
           <mechanical_reduction>2.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>-0.5</offset>
           <mechanical_reduction>-2.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
          
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::DifferentialTransmission * differential_transmission =
           dynamic_cast<transmission_interface::DifferentialTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != differential_transmission );
          
           const std::vector<double> & actuator_reduction =
           differential_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction(   );
           EXPECT_EQ(  2.0,   joint_reduction[0] );
           EXPECT_EQ(  -2.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = differential_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.5,   joint_offset[0] );
           EXPECT_EQ(  -0.5,   joint_offset[1] );
          }
          
          TEST(  DifferentialTransmissionLoaderTest,   only_mech_red_specified )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>1.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <mechanical_reduction>1.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::DifferentialTransmission * differential_transmission =
           dynamic_cast<transmission_interface::DifferentialTransmission *>(  transmission.get(   ) );
          
           const std::vector<double> & actuator_reduction =
           differential_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = differential_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   offset_and_mech_red_not_specified )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1"/>
           <actuator name="joint2_motor" role="actuator2"/>
           <joint name="joint1" role="joint1"/>
           <joint name="joint2" role="joint2"/>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::DifferentialTransmission * differential_transmission =
           dynamic_cast<transmission_interface::DifferentialTransmission *>(  transmission.get(   ) );
          
           const std::vector<double> & actuator_reduction =
           differential_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  1.0,   actuator_reduction[0] );
           EXPECT_EQ(  1.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = differential_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  DifferentialTransmissionLoaderTest,   mechanical_reduction_not_a_number )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>one</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>two</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>three</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <mechanical_reduction>four</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::DifferentialTransmission * differential_transmission =
           dynamic_cast<transmission_interface::DifferentialTransmission *>(  transmission.get(   ) );
          
           // default kicks in for ill-defined values
           const std::vector<double> & actuator_reduction =
           differential_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  1.0,   actuator_reduction[0] );
           EXPECT_EQ(  1.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = differential_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  DifferentialTransmissionLoaderTest,   offset_ill_defined )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>two</offset> <!-- Not a number -->
           <mechanical_reduction>2.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>three</offset> <!-- Not a number -->
           <mechanical_reduction>-2.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::DifferentialTransmission * differential_transmission =
           dynamic_cast<transmission_interface::DifferentialTransmission *>(  transmission.get(   ) );
          
           // default kicks in for ill-defined values
           const std::vector<double> & actuator_reduction =
           differential_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction(   );
           EXPECT_EQ(  2.0,   joint_reduction[0] );
           EXPECT_EQ(  -2.0,   joint_reduction[1] );
          
           // default kicks in for ill-defined values
           const std::vector<double> & joint_offset = differential_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  DifferentialTransmissionLoaderTest,   mech_red_invalid_value )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/DifferentialTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>0</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>0</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>2</offset>
           <mechanical_reduction>0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>3</offset>
           <mechanical_reduction>0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
           ASSERT_TRUE(  nullptr == transmission );
          }

ros2_control/transmission_interface/test/differential_transmission_test.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "random_generator_utils.hpp"
          #include "transmission_interface/differential_transmission.hpp"
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using testing::DoubleNear;
          using transmission_interface::ActuatorHandle;
          using transmission_interface::DifferentialTransmission;
          using transmission_interface::Exception;
          using transmission_interface::JointHandle;
          // Floating-point value comparison threshold
          const double EPS = 1e-6;
          
      34  TEST(  PreconditionsTest,   ExceptionThrowing )
          {
           const std::vector<double> reduction_good = {1.0,   1.0};
           const std::vector<double> reduction_bad1 = {0.0,   0.0};
           const std::vector<double> reduction_bad2 = {1.0,   0.0};
           const std::vector<double> reduction_bad3 = {0.0,   1.0};
           const std::vector<double> offset_good = {1.0,   1.0};
          
           // Invalid instance creation: Transmission cannot have zero reduction
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad1,   reduction_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad2,   reduction_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad3,   reduction_good ),   Exception );
          
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad1 ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad2 ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad3 ),   Exception );
          
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad1,   reduction_good,   offset_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad2,   reduction_good,   offset_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad3,   reduction_good,   offset_good ),   Exception );
          
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad1,   offset_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad2,   offset_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad3,   offset_good ),   Exception );
          
           // Invalid instance creation: Wrong parameter sizes
           const std::vector<double> reduction_bad_size = {1.0};
           const std::vector<double> & offset_bad_size = reduction_bad_size;
           EXPECT_THROW(  DifferentialTransmission(  reduction_bad_size,   reduction_good ),   Exception );
           EXPECT_THROW(  DifferentialTransmission(  reduction_good,   reduction_bad_size ),   Exception );
           EXPECT_THROW(  
           DifferentialTransmission(  reduction_good,   reduction_good,   offset_bad_size ),   Exception );
          
           // Valid instance creation
           EXPECT_NO_THROW(  DifferentialTransmission(  reduction_good,   reduction_good ) );
           EXPECT_NO_THROW(  DifferentialTransmission(  reduction_good,   reduction_good,   offset_good ) );
          }
          
      72  TEST(  PreconditionsTest,   AccessorValidation )
          {
           std::vector<double> act_reduction = {2.0,   -2.0};
           std::vector<double> jnt_reduction = {4.0,   -4.0};
           std::vector<double> jnt_offset = {1.0,   -1.0};
          
           DifferentialTransmission trans(  act_reduction,   jnt_reduction,   jnt_offset );
          
           EXPECT_EQ(  2u,   trans.num_actuators(   ) );
           EXPECT_EQ(  2u,   trans.num_joints(   ) );
           EXPECT_EQ(  2.0,   trans.get_actuator_reduction(   )[0] );
           EXPECT_EQ(  -2.0,   trans.get_actuator_reduction(   )[1] );
           EXPECT_EQ(  4.0,   trans.get_joint_reduction(   )[0] );
           EXPECT_EQ(  -4.0,   trans.get_joint_reduction(   )[1] );
           EXPECT_EQ(  1.0,   trans.get_joint_offset(   )[0] );
           EXPECT_EQ(  -1.0,   trans.get_joint_offset(   )[1] );
          }
          
      90  void testConfigureWithBadHandles(  std::string interface_name )
          {
           DifferentialTransmission trans(  {1.0,   1.0},   {1.0,   1.0} );
           double dummy;
          
           auto a1_handle = ActuatorHandle(  "act1",   interface_name,   &dummy );
           auto a2_handle = ActuatorHandle(  "act2",   interface_name,   &dummy );
           auto a3_handle = ActuatorHandle(  "act3",   interface_name,   &dummy );
           auto j1_handle = JointHandle(  "joint1",   interface_name,   &dummy );
           auto j2_handle = JointHandle(  "joint2",   interface_name,   &dummy );
           auto j3_handle = JointHandle(  "joint3",   interface_name,   &dummy );
           auto invalid_a1_handle = ActuatorHandle(  "act1",   interface_name,   nullptr );
           auto invalid_j1_handle = JointHandle(  "joint1",   interface_name,   nullptr );
          
           EXPECT_THROW(  trans.configure(  {},   {} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle,   j2_handle},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle,   j3_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle},   {a1_handle,   a2_handle,   a3_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle,   j3_handle},   {a1_handle,   a2_handle,   a3_handle} ),  
           Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle,   j2_handle},   {invalid_a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {invalid_j1_handle,   j2_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {invalid_j1_handle,   j2_handle},   {invalid_a1_handle,   a2_handle} ),   Exception );
          }
          
     123  TEST(  ConfigureTest,   FailsWithBadHandles )
          {
           testConfigureWithBadHandles(  HW_IF_POSITION );
           testConfigureWithBadHandles(  HW_IF_VELOCITY );
           testConfigureWithBadHandles(  HW_IF_EFFORT );
          }
          
     130  class TransmissionSetup : public ::testing::Test
          {
          protected:
           // Input/output transmission data
           double a_val[2];
           double j_val[2];
     136   std::vector<double *> a_vec = {&a_val[0],   &a_val[1]};
     137   std::vector<double *> j_vec = {&j_val[0],   &j_val[1]};
          };
          
          /// \brief Exercises the actuator->joint->actuator roundtrip,   which should yield the identity map.
     141  class BlackBoxTest : public TransmissionSetup
          {
          protected:
           const double EXTREMAL_VALUE = 1337.1337;
           /// \param trans Transmission instance.
           /// \param ref_val Reference value that will be transformed with the respective forward
           /// and inverse transmission transformations.
           /// \param interface_name The name of the interface to test,   position,   velocity,   etc.
     149   void testIdentityMap(  
     150   DifferentialTransmission & trans,   const std::vector<double> & ref_val,  
     151   const std::string & interface_name )
           {
           // set actuator values to reference
           a_val[0] = ref_val[0];
           a_val[1] = ref_val[1];
           // create handles and configure
           auto a1_handle = ActuatorHandle(  "act1",   interface_name,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   interface_name,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   interface_name,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   interface_name,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
          
           // actuator->joint->actuator roundtrip
           // but we also set actuator values to an extremal value
           // to ensure joint_to_actuator is not a no-op
           trans.actuator_to_joint(   );
           a_val[0] = a_val[1] = EXTREMAL_VALUE;
           trans.joint_to_actuator(   );
           EXPECT_THAT(  ref_val[0],   DoubleNear(  a_val[0],   EPS ) );
           EXPECT_THAT(  ref_val[1],   DoubleNear(  a_val[1],   EPS ) );
           }
          
           // Generate a set of transmission instances
           // with random combinations of actuator/joint reduction and joint offset.
     175   static std::vector<DifferentialTransmission> createTestInstances(  
     176   const vector<DifferentialTransmission>::size_type size )
           {
           std::vector<DifferentialTransmission> out;
           out.reserve(  size );
           // NOTE: Magic value
           RandomDoubleGenerator rand_gen(  -1000.0,   1000.0 );
          
           while (  out.size(   ) < size )
           {
           try
           {
           DifferentialTransmission trans(  
           randomVector(  2,   rand_gen ),   randomVector(  2,   rand_gen ),   randomVector(  2,   rand_gen ) );
           out.push_back(  trans );
           }
           catch (  const Exception & )
           {
           // NOTE: If by chance a perfect zero is produced by the random number generator,  
           // construction will fail
           // We swallow the exception and move on to prevent a test crash.
           }
           }
           return out;
           }
          };
          
     202  TEST_F(  BlackBoxTest,   IdentityMap )
          {
           // Transmission instances
           // NOTE: Magic value
           auto transmission_test_instances = createTestInstances(  100 );
          
           // Test different transmission configurations...
           for (  auto && transmission : transmission_test_instances )
           {
           // ...and for each transmission,   different input values
           // NOTE: Magic value
           RandomDoubleGenerator rand_gen(  -1000.0,   1000.0 );
           // NOTE: Magic value
           const unsigned int input_value_trials = 100;
           for (  unsigned int i = 0; i < input_value_trials; ++i )
           {
           vector<double> input_value = randomVector(  2,   rand_gen );
           // Test each interface type separately
           testIdentityMap(  transmission,   input_value,   HW_IF_POSITION );
           testIdentityMap(  transmission,   input_value,   HW_IF_VELOCITY );
           testIdentityMap(  transmission,   input_value,   HW_IF_EFFORT );
           }
           }
          }
          
     227  class WhiteBoxTest : public TransmissionSetup
          {
          };
          
     231  TEST_F(  WhiteBoxTest,   DontMoveJoints )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
           std::vector<double> joint_offset = {1.0,   1.0};
          
           DifferentialTransmission trans(  actuator_reduction,   joint_reduction,   joint_offset );
          
           // Actuator input (  used for effort,   velocity and position )
           *a_vec[0] = 0.0;
           *a_vec[1] = 0.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  joint_offset[0],   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  joint_offset[1],   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     280  TEST_F(  WhiteBoxTest,   MoveFirstJointOnly )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
          
           DifferentialTransmission trans(  actuator_reduction,   joint_reduction );
          
           // Actuator input (  used for effort,   velocity and position )
           *a_vec[0] = 10.0;
           *a_vec[1] = 10.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  400.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     328  TEST_F(  WhiteBoxTest,   MoveSecondJointOnly )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
          
           DifferentialTransmission trans(  actuator_reduction,   joint_reduction );
          
           // Actuator input (  used for effort,   velocity and position )
           *a_vec[0] = 10.0;
           *a_vec[1] = -10.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  400.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     376  TEST_F(  WhiteBoxTest,   MoveBothJoints )
          {
           // NOTE: We only test the actuator->joint map,  
           // as the joint->actuator map is indirectly validated in the test that
           // checks that actuator->joint->actuator == identity.
          
           std::vector<double> actuator_reduction = {10.0,   -20.0};
           std::vector<double> joint_reduction = {-2.0,   4.0};
           std::vector<double> joint_offset = {-2.0,   4.0};
          
           DifferentialTransmission trans(  actuator_reduction,   joint_reduction,   joint_offset );
          
           // Actuator input (  used for effort,   velocity and position )
           *a_vec[0] = 3.0;
           *a_vec[1] = 5.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  140.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  520.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  -0.01250,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.06875,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  -2.01250,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  4.06875,   DoubleNear(  j_val[1],   EPS ) );
           }
          }

ros2_control/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <exception>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "transmission_interface/four_bar_linkage_transmission.hpp"
          #include "transmission_interface/four_bar_linkage_transmission_loader.hpp"
          #include "transmission_interface/transmission_loader.hpp"
          
          using testing::SizeIs;
          
      31  class TransmissionPluginLoader
          {
          public:
      34   std::shared_ptr<transmission_interface::TransmissionLoader> create(  const std::string & type )
           {
           try
           {
           return class_loader_.createUniqueInstance(  type );
           }
           catch (  std::exception & ex )
           {
           std::cerr << ex.what(   ) << std::endl;
           return std::shared_ptr<transmission_interface::TransmissionLoader>(   );
           }
           }
          
          private:
           // must keep it alive because instance destroyers need it
      49   pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
           "transmission_interface",   "transmission_interface::TransmissionLoader"};
          };
          
      53  TEST(  FourBarLinkageTransmissionLoaderTest,   FullSpec )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>0.5</offset>
           <mechanical_reduction>2.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>-0.5</offset>
           <mechanical_reduction>-2.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
          
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
           dynamic_cast<transmission_interface::FourBarLinkageTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != four_bar_linkage_transmission );
          
           const std::vector<double> & actuator_reduction =
           four_bar_linkage_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction =
           four_bar_linkage_transmission->get_joint_reduction(   );
           EXPECT_EQ(  2.0,   joint_reduction[0] );
           EXPECT_EQ(  -2.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.5,   joint_offset[0] );
           EXPECT_EQ(  -0.5,   joint_offset[1] );
          }
          
          TEST(  FourBarLinkageTransmissionLoaderTest,   only_mech_red_specified )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>1.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <mechanical_reduction>1.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
           dynamic_cast<transmission_interface::FourBarLinkageTransmission *>(  transmission.get(   ) );
          
           const std::vector<double> & actuator_reduction =
           four_bar_linkage_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction =
           four_bar_linkage_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  DifferentialTransmissionLoaderTest,   offset_and_mech_red_not_specified )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1"/>
           <actuator name="joint2_motor" role="actuator2"/>
           <joint name="joint1" role="joint1"/>
           <joint name="joint2" role="joint2"/>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
           dynamic_cast<transmission_interface::FourBarLinkageTransmission *>(  transmission.get(   ) );
          
           const std::vector<double> & actuator_reduction =
           four_bar_linkage_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  1.0,   actuator_reduction[0] );
           EXPECT_EQ(  1.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction =
           four_bar_linkage_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  FourBarLinkageTransmissionLoaderTest,   mechanical_reduction_not_a_number )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>one</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>two</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>three</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <mechanical_reduction>four</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
           dynamic_cast<transmission_interface::FourBarLinkageTransmission *>(  transmission.get(   ) );
          
           // default kicks in for ill-defined values
           const std::vector<double> & actuator_reduction =
           four_bar_linkage_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  1.0,   actuator_reduction[0] );
           EXPECT_EQ(  1.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction =
           four_bar_linkage_transmission->get_joint_reduction(   );
           EXPECT_EQ(  1.0,   joint_reduction[0] );
           EXPECT_EQ(  1.0,   joint_reduction[1] );
          
           const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  FourBarLinkageTransmissionLoaderTest,   offset_ill_defined )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>50</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>-50</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>two</offset> <!-- Not a number -->
           <mechanical_reduction>2.0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>three</offset> <!-- Not a number -->
           <mechanical_reduction>-2.0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
          
           // Validate transmission
           transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
           dynamic_cast<transmission_interface::FourBarLinkageTransmission *>(  transmission.get(   ) );
          
           // default kicks in for ill-defined values
           const std::vector<double> & actuator_reduction =
           four_bar_linkage_transmission->get_actuator_reduction(   );
           EXPECT_EQ(  50.0,   actuator_reduction[0] );
           EXPECT_EQ(  -50.0,   actuator_reduction[1] );
          
           const std::vector<double> & joint_reduction =
           four_bar_linkage_transmission->get_joint_reduction(   );
           EXPECT_EQ(  2.0,   joint_reduction[0] );
           EXPECT_EQ(  -2.0,   joint_reduction[1] );
          
           // default kicks in for ill-defined values
           const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset(   );
           EXPECT_EQ(  0.0,   joint_offset[0] );
           EXPECT_EQ(  0.0,   joint_offset[1] );
          }
          
          TEST(  FourBarLinkageTransmissionLoaderTest,   mech_red_invalid_value )
          {
           // Parse transmission info
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="FullSpec" type="system">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-0.5</param>
           <param name="max">0.5</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <joint name="joint2">
           <command_interface name="position">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="position"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
           <actuator name="joint1_motor" role="actuator1">
           <mechanical_reduction>0</mechanical_reduction>
           </actuator>
           <actuator name="joint2_motor" role="actuator2">
           <mechanical_reduction>0</mechanical_reduction>
           </actuator>
           <joint name="joint1" role="joint1">
           <offset>2</offset>
           <mechanical_reduction>0</mechanical_reduction>
           </joint>
           <joint name="joint2" role="joint2">
           <offset>3</offset>
           <mechanical_reduction>0</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
            )";
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
           ASSERT_TRUE(  nullptr == transmission );
          }

ros2_control/transmission_interface/test/four_bar_linkage_transmission_test.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #include <gmock/gmock.h>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "random_generator_utils.hpp"
          #include "transmission_interface/four_bar_linkage_transmission.hpp"
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using testing::DoubleNear;
          using testing::Not;
          using transmission_interface::ActuatorHandle;
          using transmission_interface::Exception;
          using transmission_interface::FourBarLinkageTransmission;
          using transmission_interface::JointHandle;
          
          // Floating-point value comparison threshold
          const double EPS = 1e-6;
          
      38  TEST(  PreconditionsTest,   ExceptionThrowing )
          {
           const std::vector<double> reduction_good = {1.0,   1.0};
           const std::vector<double> reduction_bad1 = {0.0,   0.0};
           const std::vector<double> reduction_bad2 = {1.0,   0.0};
           const std::vector<double> reduction_bad3 = {0.0,   1.0};
           const std::vector<double> offset_good = {1.0,   1.0};
          
           // Invalid instance creation: Transmission cannot have zero reduction
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad1,   reduction_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad2,   reduction_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad3,   reduction_good ),   Exception );
          
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad1 ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad2 ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad3 ),   Exception );
          
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad1,   reduction_good,   offset_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad2,   reduction_good,   offset_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad3,   reduction_good,   offset_good ),   Exception );
          
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad1,   offset_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad2,   offset_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad3,   offset_good ),   Exception );
          
           // Invalid instance creation: Wrong parameter sizes
           const std::vector<double> reduction_bad_size = {1.0};
           const std::vector<double> & offset_bad_size = reduction_bad_size;
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_bad_size,   reduction_good ),   Exception );
           EXPECT_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_bad_size ),   Exception );
           EXPECT_THROW(  
           FourBarLinkageTransmission(  reduction_good,   reduction_good,   offset_bad_size ),   Exception );
          
           // Valid instance creation
           EXPECT_NO_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_good ) );
           EXPECT_NO_THROW(  FourBarLinkageTransmission(  reduction_good,   reduction_good,   offset_good ) );
          }
          
      76  TEST(  PreconditionsTest,   AccessorValidation )
          {
           std::vector<double> act_reduction = {2.0,   -2.0};
           std::vector<double> jnt_reduction = {4.0,   -4.0};
           std::vector<double> jnt_offset = {1.0,   -1.0};
          
           FourBarLinkageTransmission trans(  act_reduction,   jnt_reduction,   jnt_offset );
          
           EXPECT_EQ(  2u,   trans.num_actuators(   ) );
           EXPECT_EQ(  2u,   trans.num_joints(   ) );
           EXPECT_EQ(  2.0,   trans.get_actuator_reduction(   )[0] );
           EXPECT_EQ(  -2.0,   trans.get_actuator_reduction(   )[1] );
           EXPECT_EQ(  4.0,   trans.get_joint_reduction(   )[0] );
           EXPECT_EQ(  -4.0,   trans.get_joint_reduction(   )[1] );
           EXPECT_EQ(  1.0,   trans.get_joint_offset(   )[0] );
           EXPECT_EQ(  -1.0,   trans.get_joint_offset(   )[1] );
          }
          
      94  void testConfigureWithBadHandles(  std::string interface_name )
          {
           FourBarLinkageTransmission trans(  {1.0,   1.0},   {1.0,   1.0} );
           double dummy;
          
           auto a1_handle = ActuatorHandle(  "act1",   interface_name,   &dummy );
           auto a2_handle = ActuatorHandle(  "act2",   interface_name,   &dummy );
           auto a3_handle = ActuatorHandle(  "act3",   interface_name,   &dummy );
           auto j1_handle = JointHandle(  "joint1",   interface_name,   &dummy );
           auto j2_handle = JointHandle(  "joint2",   interface_name,   &dummy );
           auto j3_handle = JointHandle(  "joint3",   interface_name,   &dummy );
           auto invalid_a1_handle = ActuatorHandle(  "act1",   interface_name,   nullptr );
           auto invalid_j1_handle = JointHandle(  "joint1",   interface_name,   nullptr );
          
           EXPECT_THROW(  trans.configure(  {},   {} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle,   j2_handle},   {a1_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle,   j3_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle},   {a1_handle,   a2_handle,   a3_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {j1_handle,   j2_handle,   j3_handle},   {a1_handle,   a2_handle,   a3_handle} ),  
           Exception );
           EXPECT_THROW(  trans.configure(  {j1_handle,   j2_handle},   {invalid_a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  trans.configure(  {invalid_j1_handle,   j2_handle},   {a1_handle,   a2_handle} ),   Exception );
           EXPECT_THROW(  
           trans.configure(  {invalid_j1_handle,   j2_handle},   {invalid_a1_handle,   a2_handle} ),   Exception );
          }
          
     127  TEST(  ConfigureTest,   FailsWithBadHandles )
          {
           testConfigureWithBadHandles(  HW_IF_POSITION );
           testConfigureWithBadHandles(  HW_IF_VELOCITY );
           testConfigureWithBadHandles(  HW_IF_EFFORT );
          }
          
     134  class TransmissionSetup : public ::testing::Test
          {
          protected:
           // Input/output transmission data
           double a_val[2];
           double j_val[2];
     140   std::vector<double *> a_vec = {&a_val[0],   &a_val[1]};
     141   std::vector<double *> j_vec = {&j_val[0],   &j_val[1]};
          };
          
          /// \brief Exercises the actuator->joint->actuator roundtrip,   which should yield the identity map.
     145  class BlackBoxTest : public TransmissionSetup
          {
          protected:
           const double EXTREMAL_VALUE = 1337.1337;
           /// \param trans Transmission instance.
           /// \param ref_val Reference value that will be transformed with the respective forward
           /// and inverse transmission transformations.
           /// \param interface_name The name of the interface to test,   position,   velocity,   etc.
     153   void testIdentityMap(  
     154   FourBarLinkageTransmission & trans,   const std::vector<double> & ref_val,  
     155   const std::string & interface_name )
           {
           // set actuator values to reference
           a_val[0] = ref_val[0];
           a_val[1] = ref_val[1];
           // create handles and configure
           auto a1_handle = ActuatorHandle(  "act1",   interface_name,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   interface_name,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   interface_name,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   interface_name,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
          
           // actuator->joint->actuator roundtrip
           // but we also set actuator values to an extremal value
           // to ensure joint_to_actuator is not a no-op
           trans.actuator_to_joint(   );
           a_val[0] = a_val[1] = EXTREMAL_VALUE;
           trans.joint_to_actuator(   );
           EXPECT_THAT(  ref_val[0],   DoubleNear(  a_val[0],   EPS ) );
           EXPECT_THAT(  ref_val[1],   DoubleNear(  a_val[1],   EPS ) );
           }
          
           // Generate a set of transmission instances
           // with random combinations of actuator/joint reduction and joint offset.
     179   static std::vector<FourBarLinkageTransmission> createTestInstances(  
     180   const vector<FourBarLinkageTransmission>::size_type size )
           {
           std::vector<FourBarLinkageTransmission> out;
           out.reserve(  size );
           // NOTE: Magic value
           RandomDoubleGenerator rand_gen(  -1000.0,   1000.0 );
          
           while (  out.size(   ) < size )
           {
           try
           {
           FourBarLinkageTransmission trans(  
           randomVector(  2,   rand_gen ),   randomVector(  2,   rand_gen ),   randomVector(  2,   rand_gen ) );
           out.push_back(  trans );
           }
           catch (  const Exception & )
           {
           // NOTE: If by chance a perfect zero is produced by the random number generator,  
           // construction will fail
           // We swallow the exception and move on to prevent a test crash.
           }
           }
           return out;
           }
          };
          
     206  TEST_F(  BlackBoxTest,   IdentityMap )
          {
           // Transmission instances
           // NOTE: Magic value
           auto transmission_test_instances = createTestInstances(  100 );
          
           // Test different transmission configurations...
           for (  auto && transmission : transmission_test_instances )
           {
           // ...and for each transmission,   different input values
           // NOTE: Magic value
           RandomDoubleGenerator rand_gen(  -1000.0,   1000.0 );
           // NOTE: Magic value
           const unsigned int input_value_trials = 100;
           for (  unsigned int i = 0; i < input_value_trials; ++i )
           {
           vector<double> input_value = randomVector(  2,   rand_gen );
           // Test each interface type separately
           testIdentityMap(  transmission,   input_value,   HW_IF_POSITION );
           testIdentityMap(  transmission,   input_value,   HW_IF_VELOCITY );
           testIdentityMap(  transmission,   input_value,   HW_IF_EFFORT );
           }
           }
          }
          
     231  class WhiteBoxTest : public TransmissionSetup
          {
          };
          
     235  TEST_F(  WhiteBoxTest,   DontMoveJoints )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
           std::vector<double> joint_offset = {1.0,   1.0};
          
           FourBarLinkageTransmission trans(  actuator_reduction,   joint_reduction,   joint_offset );
          
           // Actuator input (  used for effort,   velocity and position )
           a_val[0] = 0.0;
           a_val[1] = 0.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  joint_offset[0],   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  joint_offset[1],   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     284  TEST_F(  WhiteBoxTest,   MoveFirstJointOnly )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
          
           FourBarLinkageTransmission trans(  actuator_reduction,   joint_reduction );
          
           // Effort interface
           {
           a_val[0] = 5.0;
           a_val[1] = 10.0;
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  100.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           a_val[0] = 10.0;
           a_val[1] = 5.0;
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           a_val[0] = 10.0;
           a_val[1] = 5.0;
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     334  TEST_F(  WhiteBoxTest,   MoveSecondJointOnly )
          {
           std::vector<double> actuator_reduction = {10.0,   10.0};
           std::vector<double> joint_reduction = {2.0,   2.0};
          
           FourBarLinkageTransmission trans(  actuator_reduction,   joint_reduction );
          
           // Actuator input (  used for effort,   velocity and position )
           a_val[0] = 0.0;
           a_val[1] = 10.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  200.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  0.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  0.5,   DoubleNear(  j_val[1],   EPS ) );
           }
          }
          
     382  TEST_F(  WhiteBoxTest,   MoveBothJoints )
          {
           // NOTE: We only test the actuator->joint map,  
           // as the joint->actuator map is indirectly validated in the test that
           // checks that actuator->joint->actuator == identity.
          
           std::vector<double> actuator_reduction = {10.0,   -20.0};
           std::vector<double> joint_reduction = {-2.0,   4.0};
           std::vector<double> joint_offset = {-2.0,   4.0};
          
           FourBarLinkageTransmission trans(  actuator_reduction,   joint_reduction,   joint_offset );
          
           // Actuator input (  used for effort,   velocity and position )
           a_val[0] = 3.0;
           a_val[1] = 5.0;
          
           // Effort interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_EFFORT,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_EFFORT,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  -60.0,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  -160.0,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Velocity interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_VELOCITY,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_VELOCITY,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  -0.15,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  -0.025,   DoubleNear(  j_val[1],   EPS ) );
           }
          
           // Position interface
           {
           auto a1_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   a_vec[0] );
           auto a2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   a_vec[1] );
           auto joint1_handle = JointHandle(  "joint1",   HW_IF_POSITION,   j_vec[0] );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   j_vec[1] );
           trans.configure(  {joint1_handle,   joint2_handle},   {a1_handle,   a2_handle} );
           trans.actuator_to_joint(   );
           EXPECT_THAT(  -2.15,   DoubleNear(  j_val[0],   EPS ) );
           EXPECT_THAT(  3.975,   DoubleNear(  j_val[1],   EPS ) );
           }
          }

ros2_control/transmission_interface/test/random_generator_utils.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef RANDOM_GENERATOR_UTILS_HPP_
          #define RANDOM_GENERATOR_UTILS_HPP_
          
          #include <cstdlib>
          #include <ctime>
          #include <vector>
          
          using std::vector;
          
          /// \brief Generator of pseudo-random double in the range [min_val,   max_val].
          // NOTE: Based on example code available at:
          // http://stackoverflow.com/questions/2860673/initializing-a-c-vector-to-random-values-fast
          struct RandomDoubleGenerator
          {
          public:
           RandomDoubleGenerator(  double min_val,   double max_val ) : min_val_(  min_val ),   max_val_(  max_val )
           {
           srand(  time(  nullptr ) );
           }
           double operator(   )(   )
           {
           const double range = max_val_ - min_val_;
           return rand(   ) / static_cast<double>(  RAND_MAX ) * range + min_val_;
           }
          
          private:
           double min_val_;
           double max_val_;
          };
          
          /// \brief Generator of a vector of pseudo-random doubles.
      48  vector<double> randomVector(  const vector<double>::size_type size,   RandomDoubleGenerator & generator )
          {
           vector<double> out;
           out.reserve(  size );
           for (  vector<double>::size_type i = 0; i < size; ++i )
           {
           out.push_back(  generator(   ) );
           }
           return out;
          }
          
          #endif // RANDOM_GENERATOR_UTILS_HPP_

ros2_control/transmission_interface/test/simple_transmission_loader_test.cpp

       1  // Copyright 2022 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <exception>
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/component_parser.hpp"
          #include "hardware_interface/hardware_info.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "pluginlib/class_loader.hpp"
          #include "transmission_interface/simple_transmission.hpp"
          #include "transmission_interface/simple_transmission_loader.hpp"
          #include "transmission_interface/transmission_loader.hpp"
          
          using testing::SizeIs;
          
      31  class TransmissionPluginLoader
          {
          public:
      34   std::shared_ptr<transmission_interface::TransmissionLoader> create(  const std::string & type )
           {
           try
           {
           return class_loader_.createUniqueInstance(  type );
           }
           catch (  std::exception & ex )
           {
           std::cerr << ex.what(   ) << std::endl;
           return std::shared_ptr<transmission_interface::TransmissionLoader>(   );
           }
           }
          
          private:
           // must keep it alive because instance destroyers need it
      49   pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
           "transmission_interface",   "transmission_interface::TransmissionLoader"};
          };
          
      53  TEST(  SimpleTransmissionLoaderTest,   FullSpec )
          {
           // Parse transmission info
          
           std::string urdf_to_test =
           R"(  
           <?xml version="1.0" encoding="utf-8"?>
           <!-- =================================================================================== -->
           <!-- | This document was autogenerated by xacro from minimal_robot.urdf.xacro | -->
           <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
           <!-- =================================================================================== -->
           <robot name="MinimalRobot">
           <!-- Used for fixing robot -->
           <link name="world"/>
           <joint name="base_joint" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <parent link="world"/>
           <child link="base_link"/>
           </joint>
           <link name="base_link">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="0.2" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
      84   <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="joint1" type="revolute">
           <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
           <parent link="base_link"/>
           <child link="link1"/>
           <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
           </joint>
           <link name="link1">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
           <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="joint2" type="revolute">
           <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
           <parent link="link1"/>
           <child link="link2"/>
           <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
           </joint>
           <link name="link2">
           <inertial>
           <mass value="0.01"/>
           <origin xyz="0 0 0"/>
           <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
           </inertial>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           <material name="DarkGrey">
           <color rgba="0.4 0.4 0.4 1.0"/>
           </material>
           </visual>
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <cylinder length="1" radius="0.1"/>
           </geometry>
           </collision>
           </link>
           <joint name="tool_joint" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 1"/>
           <parent link="link2"/>
           <child link="tool_link"/>
           </joint>
           <link name="tool_link">
           </link>
           <ros2_control name="RRBotModularJoint1" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>325.949</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           <ros2_control name="RRBotModularJoint2" type="actuator">
           <hardware>
           <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
           <param name="example_param_write_for_sec">1.23</param>
           <param name="example_param_read_for_sec">3</param>
           </hardware>
           <joint name="joint2">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint1">
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           <ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
           <hardware>
           <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
           <param name="example_param_read_for_sec">2</param>
           </hardware>
           <joint name="joint2">
           <state_interface name="position"/>
           </joint>
           </ros2_control>
           </robot>
            )";
          
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
           ASSERT_TRUE(  nullptr != transmission );
           ASSERT_STREQ(  infos[0].transmissions[0].joints[0].role.c_str(   ),   "joint1" );
          
           // Validate transmission
           transmission_interface::SimpleTransmission * simple_transmission =
           dynamic_cast<transmission_interface::SimpleTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != simple_transmission );
           EXPECT_EQ(  325.949,   simple_transmission->get_actuator_reduction(   ) );
           EXPECT_EQ(  0.0,   simple_transmission->get_joint_offset(   ) );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   only_mech_red_specified )
          {
           std::string urdf_to_test = R"(  
          <?xml version="1.0"?>
          <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="MinimalSpec" type="actuator">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint1" role="joint1">
           <mechanical_reduction>50</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
          </robot>
           )";
           // Parse transmission info
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
          
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
           loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
          
           std::shared_ptr<transmission_interface::Transmission> transmission;
           const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
           transmission = transmission_loader->load(  info );
           ASSERT_TRUE(  nullptr != transmission );
          
           // Validate transmission
           transmission_interface::SimpleTransmission * simple_transmission =
           dynamic_cast<transmission_interface::SimpleTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != simple_transmission );
           EXPECT_EQ(  50.0,   simple_transmission->get_actuator_reduction(   ) );
           EXPECT_EQ(  0.0,   simple_transmission->get_joint_offset(   ) );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   offset_and_mech_red_not_specified )
          {
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="InvalidSpec" type="actuator">
           <joint name="joint1">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission1">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint1" role="joint1">
           <!-- Unspecified element -->
           </joint>
           </transmission>
           </ros2_control>
           </robot>
           )";
           // Parse transmission info
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
           transmission_loader = loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const auto trans = infos[0].transmissions[0];
           transmission = transmission_loader->load(  trans );
           ASSERT_TRUE(  nullptr != transmission );
           transmission_interface::SimpleTransmission * simple_transmission =
           dynamic_cast<transmission_interface::SimpleTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != simple_transmission );
           EXPECT_EQ(  1.0,   simple_transmission->get_actuator_reduction(   ) );
           EXPECT_EQ(  0.0,   simple_transmission->get_joint_offset(   ) );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   mechanical_reduction_not_a_number )
          {
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="InvalidSpec" type="actuator">
           <joint name="joint2">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission2">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint2" role="joint1">
           <mechanical_reduction>fifty</mechanical_reduction> <!-- Not a number -->
           </joint>
           </transmission>
           </ros2_control>
           </robot>
           )";
           // Parse transmission info
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
           transmission_loader = loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const auto trans = infos[0].transmissions[0];
           transmission = transmission_loader->load(  trans );
           ASSERT_TRUE(  nullptr != transmission );
           transmission_interface::SimpleTransmission * simple_transmission =
           dynamic_cast<transmission_interface::SimpleTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != simple_transmission );
           // default kicks in for ill-defined values
           EXPECT_EQ(  1.0,   simple_transmission->get_actuator_reduction(   ) );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   offset_ill_defined )
          {
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="InvalidSpec" type="actuator">
           <joint name="joint3">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission3">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint3" role="joint1">
           <offset>three</offset> <!-- Not a number -->
           <mechanical_reduction>50</mechanical_reduction>
           </joint>
           </transmission>
           </ros2_control>
           </robot>
           )";
           // Parse transmission info
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
           transmission_loader = loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const auto trans = infos[0].transmissions[0];
           transmission = transmission_loader->load(  trans );
           ASSERT_TRUE(  nullptr != transmission );
           transmission_interface::SimpleTransmission * simple_transmission =
           dynamic_cast<transmission_interface::SimpleTransmission *>(  transmission.get(   ) );
           ASSERT_TRUE(  nullptr != simple_transmission );
           // default kicks in for ill-defined values
           EXPECT_EQ(  0.0,   simple_transmission->get_joint_offset(   ) );
           EXPECT_EQ(  50.0,   simple_transmission->get_actuator_reduction(   ) );
          }
          
          TEST(  SimpleTransmissionLoaderTest,   mech_red_invalid_value )
          {
           std::string urdf_to_test = R"(  
           <?xml version="1.0"?>
           <robot name="robot" xmlns="http://www.ros.org">
           <ros2_control name="InvalidSpec" type="actuator">
           <joint name="joint4">
           <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
           </command_interface>
           <state_interface name="velocity"/>
           </joint>
           <transmission name="transmission4">
           <plugin>transmission_interface/SimpleTransmission</plugin>
           <joint name="joint4" role="joint1">
           <mechanical_reduction>0</mechanical_reduction> <!-- Invalid value -->
           </joint>
           </transmission>
           </ros2_control>
           </robot>
           )";
           // Parse transmission info
           std::vector<hardware_interface::HardwareInfo> infos =
           hardware_interface::parse_control_resources_from_urdf(  urdf_to_test );
           ASSERT_THAT(  infos[0].transmissions,   SizeIs(  1 ) );
           // Transmission loader
           TransmissionPluginLoader loader;
           std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
           transmission_loader = loader.create(  infos[0].transmissions[0].type );
           ASSERT_TRUE(  nullptr != transmission_loader );
           std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
           const auto trans = infos[0].transmissions[0];
           transmission = transmission_loader->load(  trans );
           ASSERT_TRUE(  nullptr == transmission );
          }

ros2_control/transmission_interface/test/simple_transmission_test.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          #include <gmock/gmock.h>
          #include <string>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "transmission_interface/simple_transmission.hpp"
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using std::vector;
          using transmission_interface::ActuatorHandle;
          using transmission_interface::Exception;
          using transmission_interface::JointHandle;
          using transmission_interface::SimpleTransmission;
          
          // Floating-point value comparison threshold
          const double EPS = 1e-6;
          
      33  TEST(  PreconditionsTest,   ExceptionThrownWithInvalidParameters )
          {
           // Invalid instance creation: Transmission cannot have zero reduction
           EXPECT_THROW(  SimpleTransmission(  0.0 ),   Exception );
           EXPECT_THROW(  SimpleTransmission(  0.0,   1.0 ),   Exception );
           EXPECT_THROW(  SimpleTransmission(  0.0,   -1.0 ),   Exception );
          }
          
      41  TEST(  PreconditionsTest,   NoExceptionsWithValidParameters )
          {
           EXPECT_NO_THROW(  SimpleTransmission(  1.0 ) );
           EXPECT_NO_THROW(  SimpleTransmission(  1.0,   1.0 ) );
           EXPECT_NO_THROW(  SimpleTransmission(  -1.0,   1.0 ) );
           EXPECT_NO_THROW(  SimpleTransmission(  1.0,   -1.0 ) );
           EXPECT_NO_THROW(  SimpleTransmission(  -1.0,   -1.0 ) );
          }
          
      50  TEST(  PreconditionsTest,   AccessorValidation )
          {
           SimpleTransmission trans(  2.0,   -1.0 );
          
           EXPECT_EQ(  1u,   trans.num_actuators(   ) );
           EXPECT_EQ(  1u,   trans.num_joints(   ) );
           EXPECT_EQ(  2.0,   trans.get_actuator_reduction(   ) );
           EXPECT_EQ(  -1.0,   trans.get_joint_offset(   ) );
          }
          
      60  TEST(  PreconditionsTest,   ConfigureFailsWithInvalidHandles )
          {
           SimpleTransmission trans(  2.0,   -1.0 );
           double dummy;
          
           auto actuator_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   &dummy );
           auto actuator2_handle = ActuatorHandle(  "act2",   HW_IF_POSITION,   &dummy );
           auto joint_handle = JointHandle(  "joint1",   HW_IF_POSITION,   &dummy );
           auto joint2_handle = JointHandle(  "joint2",   HW_IF_POSITION,   &dummy );
          
           EXPECT_THROW(  trans.configure(  {},   {} ),   transmission_interface::Exception );
           EXPECT_THROW(  trans.configure(  {joint_handle},   {} ),   transmission_interface::Exception );
           EXPECT_THROW(  trans.configure(  {},   {actuator_handle} ),   transmission_interface::Exception );
          
           EXPECT_THROW(  
           trans.configure(  {joint_handle},   {actuator_handle,   actuator2_handle} ),  
           transmission_interface::Exception );
           EXPECT_THROW(  
           trans.configure(  {joint_handle,   joint2_handle},   {actuator_handle} ),  
           transmission_interface::Exception );
          
           auto invalid_actuator_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   nullptr );
           auto invalid_joint_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   nullptr );
           EXPECT_THROW(  
           trans.configure(  {invalid_joint_handle},   {invalid_actuator_handle} ),  
           transmission_interface::Exception );
           EXPECT_THROW(  
           trans.configure(  {},   {actuator_handle,   invalid_actuator_handle} ),  
           transmission_interface::Exception );
           EXPECT_THROW(  
           trans.configure(  {invalid_joint_handle},   {actuator_handle} ),   transmission_interface::Exception );
          }
          
      93  class TransmissionSetup
          {
          protected:
           // Input/output transmission data
           double a_val = 0.0;
           double j_val = 0.0;
          
     100   void reset_values(   )
           {
           a_val = 0.0;
           j_val = 0.0;
           }
          };
          
          /// Exercises the actuator->joint->actuator roundtrip,   which should yield the identity map.
     108  class BlackBoxTest : public TransmissionSetup,   public ::testing::TestWithParam<SimpleTransmission>
          {
          protected:
           /**
           * \param[in] trans Transmission instance.
           * \param[in] interface_name The joint/actuator interface used
           * \param[in] ref_val Reference value that will be transformed with the respective forward
           * and inverse transmission transformations.
           */
     117   void testIdentityMap(  
     118   SimpleTransmission & trans,   const std::string & interface_name,   const double ref_val )
           {
           // Effort interface
           {
           auto actuator_handle = ActuatorHandle(  "act1",   interface_name,   &a_val );
           auto joint_handle = JointHandle(  "joint1",   interface_name,   &j_val );
           trans.configure(  {joint_handle},   {actuator_handle} );
          
           a_val = ref_val;
          
           trans.actuator_to_joint(   );
           trans.joint_to_actuator(   );
           EXPECT_NEAR(  ref_val,   a_val,   EPS );
           }
           }
          };
          
     135  TEST_P(  BlackBoxTest,   IdentityMap )
          {
           // Transmission instance
           SimpleTransmission trans = GetParam(   );
          
           // Test transmission for positive,   zero,   and negative inputs
           testIdentityMap(  trans,   HW_IF_POSITION,   1.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_POSITION,   0.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_POSITION,   -1.0 );
           reset_values(   );
          
           testIdentityMap(  trans,   HW_IF_VELOCITY,   1.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_VELOCITY,   0.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_VELOCITY,   -1.0 );
           reset_values(   );
          
           testIdentityMap(  trans,   HW_IF_EFFORT,   1.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_EFFORT,   0.0 );
           reset_values(   );
           testIdentityMap(  trans,   HW_IF_EFFORT,   -1.0 );
          }
          
     162  INSTANTIATE_TEST_SUITE_P(  
           IdentityMap,   BlackBoxTest,  
           ::testing::Values(  
           SimpleTransmission(  10.0 ),   SimpleTransmission(  -10.0 ),   SimpleTransmission(  10.0,   1.0 ),  
           SimpleTransmission(  10.0,   -1.0 ),   SimpleTransmission(  -10.0,   1.0 ),  
           SimpleTransmission(  -10.0,   -1.0 ) ) );
          
     169  class WhiteBoxTest : public TransmissionSetup,   public ::testing::Test
          {
          };
          
     173  TEST_F(  WhiteBoxTest,   MoveJoint )
          {
           // NOTE: We only test the actuator->joint map
           // as the joint->actuator map is indirectly validated in the test that
           // checks that actuator->joint->actuator == identity.
          
           SimpleTransmission trans(  10.0,   1.0 );
          
           a_val = 1.0;
          
           // Effort interface
           {
           auto actuator_handle = ActuatorHandle(  "act1",   HW_IF_EFFORT,   &a_val );
           auto joint_handle = JointHandle(  "joint1",   HW_IF_EFFORT,   &j_val );
           trans.configure(  {joint_handle},   {actuator_handle} );
          
           trans.actuator_to_joint(   );
           EXPECT_NEAR(  10.0,   j_val,   EPS );
           }
          
           // Velocity interface
           {
           auto actuator_handle = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   &a_val );
           auto joint_handle = JointHandle(  "joint1",   HW_IF_VELOCITY,   &j_val );
           trans.configure(  {joint_handle},   {actuator_handle} );
          
           trans.actuator_to_joint(   );
           EXPECT_NEAR(  0.1,   j_val,   EPS );
           }
          
           // Position interface
           {
           auto actuator_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   &a_val );
           auto joint_handle = JointHandle(  "joint1",   HW_IF_POSITION,   &j_val );
           trans.configure(  {joint_handle},   {actuator_handle} );
          
           trans.actuator_to_joint(   );
           EXPECT_NEAR(  1.1,   j_val,   EPS );
           }
          
           // Mismatched interface is ignored
           {
           double unique_value = 13.37;
          
           auto actuator_handle = ActuatorHandle(  "act1",   HW_IF_POSITION,   &a_val );
           auto actuator_handle2 = ActuatorHandle(  "act1",   HW_IF_VELOCITY,   &unique_value );
           auto joint_handle = JointHandle(  "joint1",   HW_IF_POSITION,   &j_val );
           auto joint_handle2 = JointHandle(  "joint1",   HW_IF_VELOCITY,   &unique_value );
          
           trans.configure(  {joint_handle,   joint_handle2},   {actuator_handle} );
           trans.actuator_to_joint(   );
           EXPECT_NEAR(  unique_value,   13.37,   EPS );
          
           trans.configure(  {joint_handle},   {actuator_handle,   actuator_handle2} );
           trans.actuator_to_joint(   );
           EXPECT_NEAR(  unique_value,   13.37,   EPS );
           }
          }

ros2_control/transmission_interface/test/test_transmission_parser.cpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <string>
          #include <vector>
          
          #include "transmission_interface/transmission_parser.hpp"
          
          using namespace ::testing; // NOLINT
          
      23  class TestTransmissionParser : public Test
          {
          public:
           void SetUp(   ) override
           {
           valid_urdf_xml_ =
           R"(  
           <?xml version="1.0" ?>
          <!-- =================================================================================== -->
          <!-- | This document was autogenerated by xacro from rrbot.urdf.xacro | -->
          <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
          <!-- =================================================================================== -->
          <robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
           <!-- Build your comprehensive robot -->
           <link name="world"/>
           <gazebo reference="world">
           <static>true</static>
           </gazebo>
           <joint name="rrbot_fixed" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <parent link="world"/>
           <child link="rrbot_link1"/>
           </joint>
           <link name="rrbot_link1">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <geometry>
           <box size="0.1 0.1 2"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <geometry>
           <box size="0.1 0.1 2"/>
           </geometry>
           <material name="RRBOT/orange"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_joint1" type="continuous">
           <parent link="rrbot_link1"/>
           <child link="rrbot_link2"/>
           <origin rpy="0 0 0" xyz="0 0.1 1.95"/>
           <axis xyz="0 1 0"/>
           <dynamics damping="0.7"/>
           </joint>
           <link name="rrbot_link2">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           <material name="RRBOT/black"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_joint2" type="continuous">
           <parent link="rrbot_link2"/>
           <child link="rrbot_link3"/>
           <origin rpy="0 0 0" xyz="0 0.1 0.9"/>
           <axis xyz="0 1 0"/>
           <dynamics damping="0.7"/>
           </joint>
           <link name="rrbot_link3">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           <material name="RRBOT/orange"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_hokuyo_joint" type="fixed">
           <axis xyz="0 1 0"/>
           <origin rpy="0 0 0" xyz="0 0 0.975"/>
           <parent link="rrbot_link3"/>
           <child link="rrbot_hokuyo_link"/>
           </joint>
           <link name="rrbot_hokuyo_link">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.1 0.1 0.1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.1 0.1 0.1"/>
           </geometry>
           </visual>
           <inertial>
           <mass value="1e-5"/>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
           </inertial>
           </link>
           <joint name="rrbot_camera_joint" type="fixed">
           <axis xyz="0 1 0"/>
           <origin rpy="0 0 0" xyz="0.05 0 0.9"/>
           <parent link="rrbot_link3"/>
           <child link="rrbot_camera_link"/>
           </joint>
           <link name="rrbot_camera_link">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.05 0.05 0.05"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.05 0.05 0.05"/>
           </geometry>
           <material name="RRBOT/red"/>
           </visual>
           <inertial>
           <mass value="1e-5"/>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
           </inertial>
           </link>
           <transmission name="rrbot_tran1">
           <type>transmission_interface/SimpleTransmission</type>
           <joint name="rrbot_joint1">
           <hardwareInterface>PositionJointInterface</hardwareInterface>
           </joint>
           <actuator name="rrbot_motor1">
           <mechanicalReduction>1</mechanicalReduction>
           <hardwareInterface>PositionJointInterface</hardwareInterface>
           </actuator>
           </transmission>
           <transmission name="rrbot_tran2">
           <type>transmission_interface/SimpleTransmission</type>
           <joint name="rrbot_joint2">
           <hardwareInterface>VelocityJointInterface</hardwareInterface>
           </joint>
           <actuator name="rrbot_motor2">
           <mechanicalReduction>60</mechanicalReduction>
           <hardwareInterface>VelocityJointInterface</hardwareInterface>
           </actuator>
           </transmission>
           <gazebo reference="rrbot_link1">
           <material>Gazebo/Orange</material>
           </gazebo>
           <gazebo reference="rrbot_link2">
           <mu1>0.2</mu1>
           <mu2>0.2</mu2>
           <material>Gazebo/Black</material>
           </gazebo>
           <gazebo reference="rrbot_link3">
           <mu1>0.2</mu1>
           <mu2>0.2</mu2>
           <material>Gazebo/Orange</material>
           </gazebo>
           <gazebo>
           <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
           </plugin>
           </gazebo>
          </robot>
            )";
          
           wrong_urdf_xml_ =
           R"(  
           <?xml version="1.0" ?>
          <!-- =================================================================================== -->
          <!-- | This document was autogenerated by xacro from rrbot.urdf.xacro | -->
          <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
          <!-- =================================================================================== -->
          <robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
           <!-- Build your comprehensive robot -->
           <link name="world"/>
           <gazebo reference="world">
           <static>true</static>
           </gazebo>
           <joint name="rrbot_fixed" type="fixed">
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <parent link="world"/>
           <child link="rrbot_link1"/>
           </joint>
           <link name="rrbot_link1">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <geometry>
           <box size="0.1 0.1 2"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <geometry>
           <box size="0.1 0.1 2"/>
           </geometry>
           <material name="RRBOT/orange"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 1.0"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_joint1" type="continuous">
           <parent link="rrbot_link1"/>
           <child link="rrbot_link2"/>
           <origin rpy="0 0 0" xyz="0 0.1 1.95"/>
           <axis xyz="0 1 0"/>
           <dynamics damping="0.7"/>
           </joint>
           <link name="rrbot_link2">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           <material name="RRBOT/black"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_joint2" type="continuous">
           <parent link="rrbot_link2"/>
           <child link="rrbot_link3"/>
           <origin rpy="0 0 0" xyz="0 0.1 0.9"/>
           <axis xyz="0 1 0"/>
           <dynamics damping="0.7"/>
           </joint>
           <link name="rrbot_link3">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <geometry>
           <box size="0.1 0.1 1"/>
           </geometry>
           <material name="RRBOT/orange"/>
           </visual>
           <inertial>
           <origin rpy="0 0 0" xyz="0 0 0.45"/>
           <mass value="1"/>
           <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
           </inertial>
           </link>
           <joint name="rrbot_hokuyo_joint" type="fixed">
           <axis xyz="0 1 0"/>
           <origin rpy="0 0 0" xyz="0 0 0.975"/>
           <parent link="rrbot_link3"/>
           <child link="rrbot_hokuyo_link"/>
           </joint>
           <link name="rrbot_hokuyo_link">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.1 0.1 0.1"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.1 0.1 0.1"/>
           </geometry>
           </visual>
           <inertial>
           <mass value="1e-5"/>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
           </inertial>
           </link>
           <joint name="rrbot_camera_joint" type="fixed">
           <axis xyz="0 1 0"/>
           <origin rpy="0 0 0" xyz="0.05 0 0.9"/>
           <parent link="rrbot_link3"/>
           <child link="rrbot_camera_link"/>
           </joint>
           <link name="rrbot_camera_link">
           <collision>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.05 0.05 0.05"/>
           </geometry>
           </collision>
           <visual>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <geometry>
           <box size="0.05 0.05 0.05"/>
           </geometry>
           <material name="RRBOT/red"/>
           </visual>
           <inertial>
           <mass value="1e-5"/>
           <origin rpy="0 0 0" xyz="0 0 0"/>
           <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
           </inertial>
           </link>
           <transmission name="rrbot_tran1">
           <type>transmission_interface/SimpleTransmission</type>
           <actuator name="rrbot_motor1">
           <mechanicalReduction>1</mechanicalReduction>
           </actuator>
           </transmission>
           <gazebo reference="rrbot_link1">
           <material>Gazebo/Orange</material>
           </gazebo>
           <gazebo reference="rrbot_link2">
           <mu1>0.2</mu1>
           <mu2>0.2</mu2>
           <material>Gazebo/Black</material>
           </gazebo>
           <gazebo reference="rrbot_link3">
           <mu1>0.2</mu1>
           <mu2>0.2</mu2>
           <material>Gazebo/Orange</material>
           </gazebo>
           <gazebo>
           <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
           </plugin>
           </gazebo>
          </robot>
            )";
           }
          
           std::string valid_urdf_xml_;
           std::string wrong_urdf_xml_;
          };
          
          using transmission_interface::parse_transmissions_from_urdf;
          using transmission_interface::TransmissionInfo;
          
          TEST_F(  TestTransmissionParser,   successfully_parse_valid_urdf )
          {
           const auto transmissions = parse_transmissions_from_urdf(  valid_urdf_xml_ );
          
           ASSERT_THAT(  transmissions,   SizeIs(  2 ) );
          
           // first transmission
           EXPECT_EQ(  "rrbot_tran1",   transmissions[0].name );
           EXPECT_EQ(  "transmission_interface/SimpleTransmission",   transmissions[0].type );
          
           ASSERT_THAT(  transmissions[0].joints,   SizeIs(  1 ) );
           ASSERT_THAT(  transmissions[0].joints[0].interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  "rrbot_joint1",   transmissions[0].joints[0].name );
           EXPECT_EQ(  "PositionJointInterface",   transmissions[0].joints[0].interfaces[0] );
          
           ASSERT_THAT(  transmissions[0].actuators,   SizeIs(  1 ) );
           ASSERT_THAT(  transmissions[0].actuators[0].interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  "rrbot_motor1",   transmissions[0].actuators[0].name );
           EXPECT_EQ(  "PositionJointInterface",   transmissions[0].actuators[0].interfaces[0] );
           EXPECT_EQ(  1,   transmissions[0].actuators[0].mechanical_reduction );
          
           // second transmission
           EXPECT_EQ(  "rrbot_tran2",   transmissions[1].name );
           EXPECT_EQ(  "transmission_interface/SimpleTransmission",   transmissions[1].type );
          
           ASSERT_THAT(  transmissions[1].joints,   SizeIs(  1 ) );
           ASSERT_THAT(  transmissions[1].joints[0].interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  "rrbot_joint2",   transmissions[1].joints[0].name );
           EXPECT_EQ(  "VelocityJointInterface",   transmissions[1].joints[0].interfaces[0] );
          
           ASSERT_THAT(  transmissions[1].actuators,   SizeIs(  1 ) );
           ASSERT_THAT(  transmissions[1].actuators[0].interfaces,   SizeIs(  1 ) );
           EXPECT_EQ(  "rrbot_motor2",   transmissions[1].actuators[0].name );
           EXPECT_EQ(  "VelocityJointInterface",   transmissions[1].actuators[0].interfaces[0] );
           EXPECT_EQ(  60,   transmissions[1].actuators[0].mechanical_reduction );
          }
          
          TEST_F(  TestTransmissionParser,   empty_string_throws_error )
          {
           ASSERT_THROW(  transmission_interface::parse_transmissions_from_urdf(  "" ),   std::runtime_error );
          }
          
          TEST_F(  TestTransmissionParser,   empty_urdf_returns_empty )
          {
           const auto transmissions = transmission_interface::parse_transmissions_from_urdf(  
           "<?xml version=\"1.0\"?><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot>" );
           ASSERT_THAT(  transmissions,   IsEmpty(   ) );
          }
          
          TEST_F(  TestTransmissionParser,   wrong_urdf_throws_error )
          {
           EXPECT_THROW(  
           transmission_interface::parse_transmissions_from_urdf(  wrong_urdf_xml_ ),   std::runtime_error );
          }

ros2_controllers/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Bence Magyar,   Enrique Fernández,   Manuel Meraz
           */
          
          #ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
          #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
          
          #include <chrono>
          #include <cmath>
          #include <memory>
          #include <queue>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "diff_drive_controller/odometry.hpp"
          #include "diff_drive_controller/speed_limiter.hpp"
          #include "diff_drive_controller/visibility_control.h"
          #include "geometry_msgs/msg/twist.hpp"
          #include "geometry_msgs/msg/twist_stamped.hpp"
          #include "hardware_interface/handle.hpp"
          #include "nav_msgs/msg/odometry.hpp"
          #include "odometry.hpp"
          #include "rclcpp/rclcpp.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "realtime_tools/realtime_box.h"
          #include "realtime_tools/realtime_buffer.h"
          #include "realtime_tools/realtime_publisher.h"
          #include "tf2_msgs/msg/tf_message.hpp"
          
          namespace diff_drive_controller
          {
      47  class DiffDriveController : public controller_interface::ControllerInterface
          {
           using Twist = geometry_msgs::msg::TwistStamped;
          
          public:
           DIFF_DRIVE_CONTROLLER_PUBLIC
           DiffDriveController(   );
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_cleanup(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_error(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           DIFF_DRIVE_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_shutdown(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
          protected:
           struct WheelHandle
           {
           std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
           std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
           };
          
      99   const char * feedback_type(   ) const;
     100   controller_interface::CallbackReturn configure_side(  
           const std::string & side,   const std::vector<std::string> & wheel_names,  
           std::vector<WheelHandle> & registered_handles );
          
           std::vector<std::string> left_wheel_names_;
           std::vector<std::string> right_wheel_names_;
          
           std::vector<WheelHandle> registered_left_wheel_handles_;
           std::vector<WheelHandle> registered_right_wheel_handles_;
          
           struct WheelParams
           {
           size_t wheels_per_side = 0;
           double separation = 0.0; // w.r.t. the midpoint of the wheel width
           double radius = 0.0; // Assumed to be the same for both wheels
           double separation_multiplier = 1.0;
           double left_radius_multiplier = 1.0;
           double right_radius_multiplier = 1.0;
           } wheel_params_;
          
           struct OdometryParams
           {
           bool open_loop = false;
           bool position_feedback = true;
           bool enable_odom_tf = true;
           std::string base_frame_id = "base_link";
           std::string odom_frame_id = "odom";
           std::array<double,   6> pose_covariance_diagonal;
           std::array<double,   6> twist_covariance_diagonal;
           } odom_params_;
          
           Odometry odometry_;
          
     133   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
     134   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
           realtime_odometry_publisher_ = nullptr;
          
     137   std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
           nullptr;
     139   std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
           realtime_odometry_transform_publisher_ = nullptr;
          
           // Timeout to consider cmd_vel commands old
           std::chrono::milliseconds cmd_vel_timeout_{500};
          
           bool subscriber_is_active_ = false;
           rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
           rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
           velocity_command_unstamped_subscriber_ = nullptr;
          
           realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{nullptr};
          
           std::queue<Twist> previous_commands_; // last two commands
          
           // speed limiters
           SpeedLimiter limiter_linear_;
           SpeedLimiter limiter_angular_;
          
           bool publish_limited_velocity_ = false;
           std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
           std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ =
           nullptr;
          
           rclcpp::Time previous_update_timestamp_{0};
          
           // publish rate limiter
           double publish_rate_ = 50.0;
           rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(  0 );
           rclcpp::Time previous_publish_timestamp_{0};
          
           bool is_halted = false;
           bool use_stamped_vel_ = true;
          
           bool reset(   );
           void halt(   );
          };
          } // namespace diff_drive_controller
          #endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_

ros2_controllers/diff_drive_controller/include/diff_drive_controller/odometry.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Luca Marchionni
           * Author: Bence Magyar
           * Author: Enrique Fernández
           * Author: Paul Mathieu
           */
          
          #ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
          #define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
          
          #include <cmath>
          
          #include "rclcpp/time.hpp"
          #include "rcppmath/rolling_mean_accumulator.hpp"
          
          namespace diff_drive_controller
          {
      32  class Odometry
          {
          public:
      35   explicit Odometry(  size_t velocity_rolling_window_size = 10 );
          
      37   void init(  const rclcpp::Time & time );
      38   bool update(  double left_pos,   double right_pos,   const rclcpp::Time & time );
      39   bool updateFromVelocity(  double left_vel,   double right_vel,   const rclcpp::Time & time );
      40   void updateOpenLoop(  double linear,   double angular,   const rclcpp::Time & time );
      41   void resetOdometry(   );
          
      43   double getX(   ) const { return x_; }
      44   double getY(   ) const { return y_; }
      45   double getHeading(   ) const { return heading_; }
      46   double getLinear(   ) const { return linear_; }
      47   double getAngular(   ) const { return angular_; }
          
      49   void setWheelParams(  double wheel_separation,   double left_wheel_radius,   double right_wheel_radius );
      50   void setVelocityRollingWindowSize(  size_t velocity_rolling_window_size );
          
          private:
           using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
          
           void integrateRungeKutta2(  double linear,   double angular );
           void integrateExact(  double linear,   double angular );
           void resetAccumulators(   );
          
           // Current timestamp:
           rclcpp::Time timestamp_;
          
           // Current pose:
           double x_; // [m]
           double y_; // [m]
           double heading_; // [rad]
          
           // Current velocity:
           double linear_; // [m/s]
           double angular_; // [rad/s]
          
           // Wheel kinematic parameters [m]:
           double wheel_separation_;
           double left_wheel_radius_;
           double right_wheel_radius_;
          
           // Previous wheel position/state [rad]:
           double left_wheel_old_pos_;
           double right_wheel_old_pos_;
          
           // Rolling mean accumulators for the linear and angular velocities:
           size_t velocity_rolling_window_size_;
           RollingMeanAccumulator linear_accumulator_;
           RollingMeanAccumulator angular_accumulator_;
          };
          
          } // namespace diff_drive_controller
          
          #endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_

ros2_controllers/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Enrique Fernández
           */
          
          #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
          #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
          
          #include <cmath>
          
          namespace diff_drive_controller
          {
      26  class SpeedLimiter
          {
          public:
           /**
           * \brief Constructor
           * \param [in] has_velocity_limits if true,   applies velocity limits
           * \param [in] has_acceleration_limits if true,   applies acceleration limits
           * \param [in] has_jerk_limits if true,   applies jerk limits
           * \param [in] min_velocity Minimum velocity [m/s],   usually <= 0
           * \param [in] max_velocity Maximum velocity [m/s],   usually >= 0
           * \param [in] min_acceleration Minimum acceleration [m/s^2],   usually <= 0
           * \param [in] max_acceleration Maximum acceleration [m/s^2],   usually >= 0
           * \param [in] min_jerk Minimum jerk [m/s^3],   usually <= 0
           * \param [in] max_jerk Maximum jerk [m/s^3],   usually >= 0
           */
      41   SpeedLimiter(  
           bool has_velocity_limits = false,   bool has_acceleration_limits = false,  
           bool has_jerk_limits = false,   double min_velocity = NAN,   double max_velocity = NAN,  
           double min_acceleration = NAN,   double max_acceleration = NAN,   double min_jerk = NAN,  
           double max_jerk = NAN );
          
           /**
           * \brief Limit the velocity and acceleration
           * \param [in,   out] v Velocity [m/s]
           * \param [in] v0 Previous velocity to v [m/s]
           * \param [in] v1 Previous velocity to v0 [m/s]
           * \param [in] dt Time step [s]
           * \return Limiting factor (  1.0 if none )
           */
      55   double limit(  double & v,   double v0,   double v1,   double dt );
          
           /**
           * \brief Limit the velocity
           * \param [in,   out] v Velocity [m/s]
           * \return Limiting factor (  1.0 if none )
           */
      62   double limit_velocity(  double & v );
          
           /**
           * \brief Limit the acceleration
           * \param [in,   out] v Velocity [m/s]
           * \param [in] v0 Previous velocity [m/s]
           * \param [in] dt Time step [s]
           * \return Limiting factor (  1.0 if none )
           */
      71   double limit_acceleration(  double & v,   double v0,   double dt );
          
           /**
           * \brief Limit the jerk
           * \param [in,   out] v Velocity [m/s]
           * \param [in] v0 Previous velocity to v [m/s]
           * \param [in] v1 Previous velocity to v0 [m/s]
           * \param [in] dt Time step [s]
           * \return Limiting factor (  1.0 if none )
           * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
           */
      82   double limit_jerk(  double & v,   double v0,   double v1,   double dt );
          
          private:
           // Enable/Disable velocity/acceleration/jerk limits:
      86   bool has_velocity_limits_;
      87   bool has_acceleration_limits_;
      88   bool has_jerk_limits_;
          
           // Velocity limits:
           double min_velocity_;
           double max_velocity_;
          
           // Acceleration limits:
           double min_acceleration_;
           double max_acceleration_;
          
           // Jerk limits:
           double min_jerk_;
           double max_jerk_;
          };
          
          } // namespace diff_drive_controller
          
          #endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_

ros2_controllers/diff_drive_controller/include/diff_drive_controller/visibility_control.h

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_
          #define DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__(  (  dllexport ) )
          #define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define DIFF_DRIVE_CONTROLLER_EXPORT __declspec(  dllexport )
          #define DIFF_DRIVE_CONTROLLER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL
          #define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT
          #else
          #define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT
          #endif
          #define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC
          #define DIFF_DRIVE_CONTROLLER_LOCAL
          #else
          #define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define DIFF_DRIVE_CONTROLLER_IMPORT
          #if __GNUC__ >= 4
          #define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define DIFF_DRIVE_CONTROLLER_PUBLIC
          #define DIFF_DRIVE_CONTROLLER_LOCAL
          #endif
          #define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE
          #endif
          
          #endif // DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_

ros2_controllers/diff_drive_controller/src/diff_drive_controller.cpp

          // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Bence Magyar,   Enrique Fernández,   Manuel Meraz
           */
          
          #include <memory>
          #include <queue>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "diff_drive_controller/diff_drive_controller.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/logging.hpp"
          #include "tf2/LinearMath/Quaternion.h"
          
          namespace
          {
      33  constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel";
          constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped";
          constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out";
          constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
          constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
          } // namespace
          
          namespace diff_drive_controller
          {
          using namespace std::chrono_literals;
          using controller_interface::interface_configuration_type;
          using controller_interface::InterfaceConfiguration;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using lifecycle_msgs::msg::State;
          
          DiffDriveController::DiffDriveController(   ) : controller_interface::ControllerInterface(   ) {}
          
          const char * DiffDriveController::feedback_type(   ) const
          {
           return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_init(   )
          {
           try
           {
           // with the lifecycle node being initialized,   we can declare parameters
           auto_declare<std::vector<std::string>>(  "left_wheel_names",   std::vector<std::string>(   ) );
           auto_declare<std::vector<std::string>>(  "right_wheel_names",   std::vector<std::string>(   ) );
          
           auto_declare<double>(  "wheel_separation",   wheel_params_.separation );
           auto_declare<int>(  "wheels_per_side",   wheel_params_.wheels_per_side );
           auto_declare<double>(  "wheel_radius",   wheel_params_.radius );
           auto_declare<double>(  "wheel_separation_multiplier",   wheel_params_.separation_multiplier );
           auto_declare<double>(  "left_wheel_radius_multiplier",   wheel_params_.left_radius_multiplier );
           auto_declare<double>(  "right_wheel_radius_multiplier",   wheel_params_.right_radius_multiplier );
          
           auto_declare<std::string>(  "odom_frame_id",   odom_params_.odom_frame_id );
           auto_declare<std::string>(  "base_frame_id",   odom_params_.base_frame_id );
           auto_declare<std::vector<double>>(  "pose_covariance_diagonal",   std::vector<double>(   ) );
           auto_declare<std::vector<double>>(  "twist_covariance_diagonal",   std::vector<double>(   ) );
           auto_declare<bool>(  "open_loop",   odom_params_.open_loop );
           auto_declare<bool>(  "position_feedback",   odom_params_.position_feedback );
           auto_declare<bool>(  "enable_odom_tf",   odom_params_.enable_odom_tf );
          
           auto_declare<double>(  "cmd_vel_timeout",   cmd_vel_timeout_.count(   ) / 1000.0 );
           auto_declare<bool>(  "publish_limited_velocity",   publish_limited_velocity_ );
           auto_declare<int>(  "velocity_rolling_window_size",   10 );
           auto_declare<bool>(  "use_stamped_vel",   use_stamped_vel_ );
          
           auto_declare<bool>(  "linear.x.has_velocity_limits",   false );
           auto_declare<bool>(  "linear.x.has_acceleration_limits",   false );
           auto_declare<bool>(  "linear.x.has_jerk_limits",   false );
           auto_declare<double>(  "linear.x.max_velocity",   NAN );
           auto_declare<double>(  "linear.x.min_velocity",   NAN );
           auto_declare<double>(  "linear.x.max_acceleration",   NAN );
           auto_declare<double>(  "linear.x.min_acceleration",   NAN );
           auto_declare<double>(  "linear.x.max_jerk",   NAN );
           auto_declare<double>(  "linear.x.min_jerk",   NAN );
          
           auto_declare<bool>(  "angular.z.has_velocity_limits",   false );
           auto_declare<bool>(  "angular.z.has_acceleration_limits",   false );
           auto_declare<bool>(  "angular.z.has_jerk_limits",   false );
           auto_declare<double>(  "angular.z.max_velocity",   NAN );
           auto_declare<double>(  "angular.z.min_velocity",   NAN );
           auto_declare<double>(  "angular.z.max_acceleration",   NAN );
           auto_declare<double>(  "angular.z.min_acceleration",   NAN );
           auto_declare<double>(  "angular.z.max_jerk",   NAN );
           auto_declare<double>(  "angular.z.min_jerk",   NAN );
           auto_declare<double>(  "publish_rate",   publish_rate_ );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          InterfaceConfiguration DiffDriveController::command_interface_configuration(   ) const
          {
           std::vector<std::string> conf_names;
           for (  const auto & joint_name : left_wheel_names_ )
           {
           conf_names.push_back(  joint_name + "/" + HW_IF_VELOCITY );
           }
           for (  const auto & joint_name : right_wheel_names_ )
           {
           conf_names.push_back(  joint_name + "/" + HW_IF_VELOCITY );
           }
           return {interface_configuration_type::INDIVIDUAL,   conf_names};
          }
          
          InterfaceConfiguration DiffDriveController::state_interface_configuration(   ) const
          {
           std::vector<std::string> conf_names;
           for (  const auto & joint_name : left_wheel_names_ )
           {
           conf_names.push_back(  joint_name + "/" + feedback_type(   ) );
           }
           for (  const auto & joint_name : right_wheel_names_ )
           {
           conf_names.push_back(  joint_name + "/" + feedback_type(   ) );
           }
           return {interface_configuration_type::INDIVIDUAL,   conf_names};
          }
          
          controller_interface::return_type DiffDriveController::update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           auto logger = get_node(   )->get_logger(   );
           if (  get_state(   ).id(   ) == State::PRIMARY_STATE_INACTIVE )
           {
           if (  !is_halted )
           {
           halt(   );
           is_halted = true;
           }
           return controller_interface::return_type::OK;
           }
          
           std::shared_ptr<Twist> last_command_msg;
           received_velocity_msg_ptr_.get(  last_command_msg );
          
           if (  last_command_msg == nullptr )
           {
           RCLCPP_WARN(  logger,   "Velocity message received was a nullptr." );
           return controller_interface::return_type::ERROR;
           }
          
           const auto age_of_last_command = time - last_command_msg->header.stamp;
           // Brake if cmd_vel has timeout,   override the stored command
           if (  age_of_last_command > cmd_vel_timeout_ )
           {
           last_command_msg->twist.linear.x = 0.0;
           last_command_msg->twist.angular.z = 0.0;
           }
          
           // command may be limited further by SpeedLimit,  
           // without affecting the stored twist command
           Twist command = *last_command_msg;
           double & linear_command = command.twist.linear.x;
           double & angular_command = command.twist.angular.z;
          
           previous_update_timestamp_ = time;
          
           // Apply (  possibly new ) multipliers:
           const auto wheels = wheel_params_;
           const double wheel_separation = wheels.separation_multiplier * wheels.separation;
           const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius;
           const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius;
          
           if (  odom_params_.open_loop )
           {
           odometry_.updateOpenLoop(  linear_command,   angular_command,   time );
           }
           else
           {
           double left_feedback_mean = 0.0;
           double right_feedback_mean = 0.0;
           for (  size_t index = 0; index < wheels.wheels_per_side; ++index )
           {
           const double left_feedback = registered_left_wheel_handles_[index].feedback.get(   ).get_value(   );
           const double right_feedback =
           registered_right_wheel_handles_[index].feedback.get(   ).get_value(   );
          
           if (  std::isnan(  left_feedback ) || std::isnan(  right_feedback ) )
           {
           RCLCPP_ERROR(  
           logger,   "Either the left or right wheel %s is invalid for index [%zu]",   feedback_type(   ),  
           index );
           return controller_interface::return_type::ERROR;
           }
          
           left_feedback_mean += left_feedback;
           right_feedback_mean += right_feedback;
           }
           left_feedback_mean /= wheels.wheels_per_side;
           right_feedback_mean /= wheels.wheels_per_side;
          
           if (  odom_params_.position_feedback )
           {
           odometry_.update(  left_feedback_mean,   right_feedback_mean,   time );
           }
           else
           {
           odometry_.updateFromVelocity(  
           left_feedback_mean * period.seconds(   ),   right_feedback_mean * period.seconds(   ),   time );
           }
           }
          
           tf2::Quaternion orientation;
           orientation.setRPY(  0.0,   0.0,   odometry_.getHeading(   ) );
          
           if (  previous_publish_timestamp_ + publish_period_ < time )
           {
           previous_publish_timestamp_ += publish_period_;
          
           if (  realtime_odometry_publisher_->trylock(   ) )
           {
           auto & odometry_message = realtime_odometry_publisher_->msg_;
           odometry_message.header.stamp = time;
           odometry_message.pose.pose.position.x = odometry_.getX(   );
           odometry_message.pose.pose.position.y = odometry_.getY(   );
           odometry_message.pose.pose.orientation.x = orientation.x(   );
           odometry_message.pose.pose.orientation.y = orientation.y(   );
           odometry_message.pose.pose.orientation.z = orientation.z(   );
           odometry_message.pose.pose.orientation.w = orientation.w(   );
           odometry_message.twist.twist.linear.x = odometry_.getLinear(   );
           odometry_message.twist.twist.angular.z = odometry_.getAngular(   );
           realtime_odometry_publisher_->unlockAndPublish(   );
           }
          
           if (  odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock(   ) )
           {
           auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(   );
           transform.header.stamp = time;
           transform.transform.translation.x = odometry_.getX(   );
           transform.transform.translation.y = odometry_.getY(   );
           transform.transform.rotation.x = orientation.x(   );
           transform.transform.rotation.y = orientation.y(   );
           transform.transform.rotation.z = orientation.z(   );
           transform.transform.rotation.w = orientation.w(   );
           realtime_odometry_transform_publisher_->unlockAndPublish(   );
           }
           }
          
           auto & last_command = previous_commands_.back(   ).twist;
           auto & second_to_last_command = previous_commands_.front(   ).twist;
           limiter_linear_.limit(  
           linear_command,   last_command.linear.x,   second_to_last_command.linear.x,   period.seconds(   ) );
           limiter_angular_.limit(  
           angular_command,   last_command.angular.z,   second_to_last_command.angular.z,   period.seconds(   ) );
          
           previous_commands_.pop(   );
           previous_commands_.emplace(  command );
          
           // Publish limited velocity
           if (  publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock(   ) )
           {
           auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
           limited_velocity_command.header.stamp = time;
           limited_velocity_command.twist = command.twist;
           realtime_limited_velocity_publisher_->unlockAndPublish(   );
           }
          
           // Compute wheels velocities:
           const double velocity_left =
           (  linear_command - angular_command * wheel_separation / 2.0 ) / left_wheel_radius;
           const double velocity_right =
           (  linear_command + angular_command * wheel_separation / 2.0 ) / right_wheel_radius;
          
           // Set wheels velocities:
           for (  size_t index = 0; index < wheels.wheels_per_side; ++index )
           {
           registered_left_wheel_handles_[index].velocity.get(   ).set_value(  velocity_left );
           registered_right_wheel_handles_[index].velocity.get(   ).set_value(  velocity_right );
           }
          
           return controller_interface::return_type::OK;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_configure(  
           const rclcpp_lifecycle::State & )
          {
           auto logger = get_node(   )->get_logger(   );
          
           // update parameters
           left_wheel_names_ = get_node(   )->get_parameter(  "left_wheel_names" ).as_string_array(   );
           right_wheel_names_ = get_node(   )->get_parameter(  "right_wheel_names" ).as_string_array(   );
          
           if (  left_wheel_names_.size(   ) != right_wheel_names_.size(   ) )
           {
           RCLCPP_ERROR(  
           logger,   "The number of left wheels [%zu] and the number of right wheels [%zu] are different",  
           left_wheel_names_.size(   ),   right_wheel_names_.size(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           if (  left_wheel_names_.empty(   ) )
           {
           RCLCPP_ERROR(  logger,   "Wheel names parameters are empty!" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           wheel_params_.separation = get_node(   )->get_parameter(  "wheel_separation" ).as_double(   );
           wheel_params_.wheels_per_side =
           static_cast<size_t>(  get_node(   )->get_parameter(  "wheels_per_side" ).as_int(   ) );
           wheel_params_.radius = get_node(   )->get_parameter(  "wheel_radius" ).as_double(   );
           wheel_params_.separation_multiplier =
           get_node(   )->get_parameter(  "wheel_separation_multiplier" ).as_double(   );
           wheel_params_.left_radius_multiplier =
           get_node(   )->get_parameter(  "left_wheel_radius_multiplier" ).as_double(   );
           wheel_params_.right_radius_multiplier =
           get_node(   )->get_parameter(  "right_wheel_radius_multiplier" ).as_double(   );
          
           const auto wheels = wheel_params_;
          
           const double wheel_separation = wheels.separation_multiplier * wheels.separation;
           const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius;
           const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius;
          
           odometry_.setWheelParams(  wheel_separation,   left_wheel_radius,   right_wheel_radius );
           odometry_.setVelocityRollingWindowSize(  
           get_node(   )->get_parameter(  "velocity_rolling_window_size" ).as_int(   ) );
          
           odom_params_.odom_frame_id = get_node(   )->get_parameter(  "odom_frame_id" ).as_string(   );
           odom_params_.base_frame_id = get_node(   )->get_parameter(  "base_frame_id" ).as_string(   );
          
           auto pose_diagonal = get_node(   )->get_parameter(  "pose_covariance_diagonal" ).as_double_array(   );
           std::copy(  
           pose_diagonal.begin(   ),   pose_diagonal.end(   ),   odom_params_.pose_covariance_diagonal.begin(   ) );
          
           auto twist_diagonal = get_node(   )->get_parameter(  "twist_covariance_diagonal" ).as_double_array(   );
           std::copy(  
           twist_diagonal.begin(   ),   twist_diagonal.end(   ),   odom_params_.twist_covariance_diagonal.begin(   ) );
          
           odom_params_.open_loop = get_node(   )->get_parameter(  "open_loop" ).as_bool(   );
           odom_params_.position_feedback = get_node(   )->get_parameter(  "position_feedback" ).as_bool(   );
           odom_params_.enable_odom_tf = get_node(   )->get_parameter(  "enable_odom_tf" ).as_bool(   );
          
           cmd_vel_timeout_ = std::chrono::milliseconds{
           static_cast<int>(  get_node(   )->get_parameter(  "cmd_vel_timeout" ).as_double(   ) * 1000.0 )};
           publish_limited_velocity_ = get_node(   )->get_parameter(  "publish_limited_velocity" ).as_bool(   );
           use_stamped_vel_ = get_node(   )->get_parameter(  "use_stamped_vel" ).as_bool(   );
          
           try
           {
           limiter_linear_ = SpeedLimiter(  
           get_node(   )->get_parameter(  "linear.x.has_velocity_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "linear.x.has_acceleration_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "linear.x.has_jerk_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "linear.x.min_velocity" ).as_double(   ),  
           get_node(   )->get_parameter(  "linear.x.max_velocity" ).as_double(   ),  
           get_node(   )->get_parameter(  "linear.x.min_acceleration" ).as_double(   ),  
           get_node(   )->get_parameter(  "linear.x.max_acceleration" ).as_double(   ),  
           get_node(   )->get_parameter(  "linear.x.min_jerk" ).as_double(   ),  
           get_node(   )->get_parameter(  "linear.x.max_jerk" ).as_double(   ) );
           }
           catch (  const std::runtime_error & e )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Error configuring linear speed limiter: %s",   e.what(   ) );
           }
          
           try
           {
           limiter_angular_ = SpeedLimiter(  
           get_node(   )->get_parameter(  "angular.z.has_velocity_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "angular.z.has_acceleration_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "angular.z.has_jerk_limits" ).as_bool(   ),  
           get_node(   )->get_parameter(  "angular.z.min_velocity" ).as_double(   ),  
           get_node(   )->get_parameter(  "angular.z.max_velocity" ).as_double(   ),  
           get_node(   )->get_parameter(  "angular.z.min_acceleration" ).as_double(   ),  
           get_node(   )->get_parameter(  "angular.z.max_acceleration" ).as_double(   ),  
           get_node(   )->get_parameter(  "angular.z.min_jerk" ).as_double(   ),  
           get_node(   )->get_parameter(  "angular.z.max_jerk" ).as_double(   ) );
           }
           catch (  const std::runtime_error & e )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Error configuring angular speed limiter: %s",   e.what(   ) );
           }
          
           if (  !reset(   ) )
           {
           return controller_interface::CallbackReturn::ERROR;
           }
          
           // left and right sides are both equal at this point
           wheel_params_.wheels_per_side = left_wheel_names_.size(   );
          
           if (  publish_limited_velocity_ )
           {
           limited_velocity_publisher_ =
           get_node(   )->create_publisher<Twist>(  DEFAULT_COMMAND_OUT_TOPIC,   rclcpp::SystemDefaultsQoS(   ) );
           realtime_limited_velocity_publisher_ =
           std::make_shared<realtime_tools::RealtimePublisher<Twist>>(  limited_velocity_publisher_ );
           }
          
           const Twist empty_twist;
           received_velocity_msg_ptr_.set(  std::make_shared<Twist>(  empty_twist ) );
          
           // Fill last two commands with default constructed commands
           previous_commands_.emplace(  empty_twist );
           previous_commands_.emplace(  empty_twist );
          
           // initialize command subscriber
           if (  use_stamped_vel_ )
           {
           velocity_command_subscriber_ = get_node(   )->create_subscription<Twist>(  
           DEFAULT_COMMAND_TOPIC,   rclcpp::SystemDefaultsQoS(   ),  
           [this](  const std::shared_ptr<Twist> msg ) -> void {
           if (  !subscriber_is_active_ )
           {
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),   "Can't accept new commands. subscriber is inactive" );
           return;
           }
           if (  (  msg->header.stamp.sec == 0 ) && (  msg->header.stamp.nanosec == 0 ) )
           {
           RCLCPP_WARN_ONCE(  
           get_node(   )->get_logger(   ),  
           "Received TwistStamped with zero timestamp,   setting it to current "
           "time,   this message will only be shown once" );
           msg->header.stamp = get_node(   )->get_clock(   )->now(   );
           }
           received_velocity_msg_ptr_.set(  std::move(  msg ) );
           } );
           }
           else
           {
           velocity_command_unstamped_subscriber_ =
           get_node(   )->create_subscription<geometry_msgs::msg::Twist>(  
           DEFAULT_COMMAND_UNSTAMPED_TOPIC,   rclcpp::SystemDefaultsQoS(   ),  
           [this](  const std::shared_ptr<geometry_msgs::msg::Twist> msg ) -> void {
           if (  !subscriber_is_active_ )
           {
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),   "Can't accept new commands. subscriber is inactive" );
           return;
           }
          
           // Write fake header in the stored stamped command
           std::shared_ptr<Twist> twist_stamped;
           received_velocity_msg_ptr_.get(  twist_stamped );
           twist_stamped->twist = *msg;
           twist_stamped->header.stamp = get_node(   )->get_clock(   )->now(   );
           } );
           }
          
           // initialize odometry publisher and messasge
           odometry_publisher_ = get_node(   )->create_publisher<nav_msgs::msg::Odometry>(  
           DEFAULT_ODOMETRY_TOPIC,   rclcpp::SystemDefaultsQoS(   ) );
           realtime_odometry_publisher_ =
           std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(  
           odometry_publisher_ );
          
           auto & odometry_message = realtime_odometry_publisher_->msg_;
           odometry_message.header.frame_id = odom_params_.odom_frame_id;
           odometry_message.child_frame_id = odom_params_.base_frame_id;
          
           // limit the publication on the topics /odom and /tf
           publish_rate_ = get_node(   )->get_parameter(  "publish_rate" ).as_double(   );
           publish_period_ = rclcpp::Duration::from_seconds(  1.0 / publish_rate_ );
           previous_publish_timestamp_ = get_node(   )->get_clock(   )->now(   );
          
           // initialize odom values zeros
           odometry_message.twist =
           geometry_msgs::msg::TwistWithCovariance(  rosidl_runtime_cpp::MessageInitialization::ALL );
          
           constexpr size_t NUM_DIMENSIONS = 6;
           for (  size_t index = 0; index < 6; ++index )
           {
           // 0,   7,   14,   21,   28,   35
           const size_t diagonal_index = NUM_DIMENSIONS * index + index;
           odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index];
           odometry_message.twist.covariance[diagonal_index] =
           odom_params_.twist_covariance_diagonal[index];
           }
          
           // initialize transform publisher and message
           odometry_transform_publisher_ = get_node(   )->create_publisher<tf2_msgs::msg::TFMessage>(  
           DEFAULT_TRANSFORM_TOPIC,   rclcpp::SystemDefaultsQoS(   ) );
           realtime_odometry_transform_publisher_ =
           std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(  
           odometry_transform_publisher_ );
          
           // keeping track of odom and base_link transforms only
           auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
           odometry_transform_message.transforms.resize(  1 );
           odometry_transform_message.transforms.front(   ).header.frame_id = odom_params_.odom_frame_id;
           odometry_transform_message.transforms.front(   ).child_frame_id = odom_params_.base_frame_id;
          
           previous_update_timestamp_ = get_node(   )->get_clock(   )->now(   );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_activate(  
           const rclcpp_lifecycle::State & )
          {
           const auto left_result =
           configure_side(  "left",   left_wheel_names_,   registered_left_wheel_handles_ );
           const auto right_result =
           configure_side(  "right",   right_wheel_names_,   registered_right_wheel_handles_ );
          
           if (  
           left_result == controller_interface::CallbackReturn::ERROR ||
           right_result == controller_interface::CallbackReturn::ERROR )
           {
           return controller_interface::CallbackReturn::ERROR;
           }
          
           if (  registered_left_wheel_handles_.empty(   ) || registered_right_wheel_handles_.empty(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "Either left wheel interfaces,   right wheel interfaces are non existent" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           is_halted = false;
           subscriber_is_active_ = true;
          
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "Subscriber and publisher are now active." );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_deactivate(  
           const rclcpp_lifecycle::State & )
          {
           subscriber_is_active_ = false;
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_cleanup(  
           const rclcpp_lifecycle::State & )
          {
           if (  !reset(   ) )
           {
           return controller_interface::CallbackReturn::ERROR;
           }
          
           received_velocity_msg_ptr_.set(  std::make_shared<Twist>(   ) );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_error(  const rclcpp_lifecycle::State & )
          {
           if (  !reset(   ) )
           {
           return controller_interface::CallbackReturn::ERROR;
           }
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          bool DiffDriveController::reset(   )
          {
           odometry_.resetOdometry(   );
          
           // release the old queue
           std::queue<Twist> empty;
           std::swap(  previous_commands_,   empty );
          
           registered_left_wheel_handles_.clear(   );
           registered_right_wheel_handles_.clear(   );
          
           subscriber_is_active_ = false;
           velocity_command_subscriber_.reset(   );
           velocity_command_unstamped_subscriber_.reset(   );
          
           received_velocity_msg_ptr_.set(  nullptr );
           is_halted = false;
           return true;
          }
          
          controller_interface::CallbackReturn DiffDriveController::on_shutdown(  
           const rclcpp_lifecycle::State & )
          {
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          void DiffDriveController::halt(   )
          {
           const auto halt_wheels = [](  auto & wheel_handles ) {
           for (  const auto & wheel_handle : wheel_handles )
           {
           wheel_handle.velocity.get(   ).set_value(  0.0 );
           }
           };
          
           halt_wheels(  registered_left_wheel_handles_ );
           halt_wheels(  registered_right_wheel_handles_ );
          }
          
          controller_interface::CallbackReturn DiffDriveController::configure_side(  
           const std::string & side,   const std::vector<std::string> & wheel_names,  
           std::vector<WheelHandle> & registered_handles )
          {
           auto logger = get_node(   )->get_logger(   );
          
           if (  wheel_names.empty(   ) )
           {
           RCLCPP_ERROR(  logger,   "No '%s' wheel names specified",   side.c_str(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           // register handles
           registered_handles.reserve(  wheel_names.size(   ) );
           for (  const auto & wheel_name : wheel_names )
           {
           const auto interface_name = feedback_type(   );
           const auto state_handle = std::find_if(  
           state_interfaces_.cbegin(   ),   state_interfaces_.cend(   ),  
           [&wheel_name,   &interface_name](  const auto & interface ) {
           return interface.get_prefix_name(   ) == wheel_name &&
           interface.get_interface_name(   ) == interface_name;
           } );
          
           if (  state_handle == state_interfaces_.cend(   ) )
           {
           RCLCPP_ERROR(  logger,   "Unable to obtain joint state handle for %s",   wheel_name.c_str(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           const auto command_handle = std::find_if(  
           command_interfaces_.begin(   ),   command_interfaces_.end(   ),  
           [&wheel_name](  const auto & interface ) {
           return interface.get_prefix_name(   ) == wheel_name &&
           interface.get_interface_name(   ) == HW_IF_VELOCITY;
           } );
          
           if (  command_handle == command_interfaces_.end(   ) )
           {
           RCLCPP_ERROR(  logger,   "Unable to obtain joint command handle for %s",   wheel_name.c_str(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           registered_handles.emplace_back(  
           WheelHandle{std::ref(  *state_handle ),   std::ref(  *command_handle )} );
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          } // namespace diff_drive_controller
          
          #include "class_loader/register_macro.hpp"
          
          CLASS_LOADER_REGISTER_CLASS(  
           diff_drive_controller::DiffDriveController,   controller_interface::ControllerInterface )

ros2_controllers/diff_drive_controller/src/odometry.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Enrique Fernández
           */
          
          #include "diff_drive_controller/odometry.hpp"
          
          namespace diff_drive_controller
          {
      23  Odometry::Odometry(  size_t velocity_rolling_window_size )
          : timestamp_(  0.0 ),  
           x_(  0.0 ),  
           y_(  0.0 ),  
           heading_(  0.0 ),  
           linear_(  0.0 ),  
           angular_(  0.0 ),  
           wheel_separation_(  0.0 ),  
           left_wheel_radius_(  0.0 ),  
           right_wheel_radius_(  0.0 ),  
           left_wheel_old_pos_(  0.0 ),  
           right_wheel_old_pos_(  0.0 ),  
           velocity_rolling_window_size_(  velocity_rolling_window_size ),  
           linear_accumulator_(  velocity_rolling_window_size ),  
           angular_accumulator_(  velocity_rolling_window_size )
          {
          }
          
      41  void Odometry::init(  const rclcpp::Time & time )
          {
           // Reset accumulators and timestamp:
           resetAccumulators(   );
           timestamp_ = time;
          }
          
      48  bool Odometry::update(  double left_pos,   double right_pos,   const rclcpp::Time & time )
          {
           // We cannot estimate the speed with very small time intervals:
           const double dt = time.seconds(   ) - timestamp_.seconds(   );
           if (  dt < 0.0001 )
           {
           return false; // Interval too small to integrate with
           }
          
           // Get current wheel joint positions:
           const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
           const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
          
           // Estimate velocity of wheels using old and current position:
           const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
           const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
          
           // Update old position with current:
           left_wheel_old_pos_ = left_wheel_cur_pos;
           right_wheel_old_pos_ = right_wheel_cur_pos;
          
           updateFromVelocity(  left_wheel_est_vel,   right_wheel_est_vel,   time );
          
           return true;
          }
          
      74  bool Odometry::updateFromVelocity(  double left_vel,   double right_vel,   const rclcpp::Time & time )
          {
           const double dt = time.seconds(   ) - timestamp_.seconds(   );
          
           // Compute linear and angular diff:
           const double linear = (  left_vel + right_vel ) * 0.5;
           // Now there is a bug about scout angular velocity
           const double angular = (  right_vel - left_vel ) / wheel_separation_;
          
           // Integrate odometry:
           integrateExact(  linear,   angular );
          
           timestamp_ = time;
          
           // Estimate speeds using a rolling mean to filter them out:
           linear_accumulator_.accumulate(  linear / dt );
           angular_accumulator_.accumulate(  angular / dt );
          
           linear_ = linear_accumulator_.getRollingMean(   );
           angular_ = angular_accumulator_.getRollingMean(   );
          
           return true;
          }
          
      98  void Odometry::updateOpenLoop(  double linear,   double angular,   const rclcpp::Time & time )
          {
           /// Save last linear and angular velocity:
           linear_ = linear;
           angular_ = angular;
          
           /// Integrate odometry:
           const double dt = time.seconds(   ) - timestamp_.seconds(   );
           timestamp_ = time;
           integrateExact(  linear * dt,   angular * dt );
          }
          
     110  void Odometry::resetOdometry(   )
          {
           x_ = 0.0;
           y_ = 0.0;
           heading_ = 0.0;
          }
          
     117  void Odometry::setWheelParams(  
           double wheel_separation,   double left_wheel_radius,   double right_wheel_radius )
          {
           wheel_separation_ = wheel_separation;
           left_wheel_radius_ = left_wheel_radius;
           right_wheel_radius_ = right_wheel_radius;
          }
          
     125  void Odometry::setVelocityRollingWindowSize(  size_t velocity_rolling_window_size )
          {
           velocity_rolling_window_size_ = velocity_rolling_window_size;
          
           resetAccumulators(   );
          }
          
     132  void Odometry::integrateRungeKutta2(  double linear,   double angular )
          {
           const double direction = heading_ + angular * 0.5;
          
           /// Runge-Kutta 2nd order integration:
           x_ += linear * cos(  direction );
           y_ += linear * sin(  direction );
           heading_ += angular;
          }
          
     142  void Odometry::integrateExact(  double linear,   double angular )
          {
           if (  fabs(  angular ) < 1e-6 )
           {
           integrateRungeKutta2(  linear,   angular );
           }
           else
           {
           /// Exact integration (  should solve problems when angular is zero ):
           const double heading_old = heading_;
           const double r = linear / angular;
           heading_ += angular;
           x_ += r * (  sin(  heading_ ) - sin(  heading_old ) );
           y_ += -r * (  cos(  heading_ ) - cos(  heading_old ) );
           }
          }
          
     159  void Odometry::resetAccumulators(   )
          {
           linear_accumulator_ = RollingMeanAccumulator(  velocity_rolling_window_size_ );
           angular_accumulator_ = RollingMeanAccumulator(  velocity_rolling_window_size_ );
          }
          
          } // namespace diff_drive_controller

ros2_controllers/diff_drive_controller/src/speed_limiter.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Enrique Fernández
           */
          
          #include <algorithm>
          #include <stdexcept>
          
          #include "diff_drive_controller/speed_limiter.hpp"
          #include "rcppmath/clamp.hpp"
          
          namespace diff_drive_controller
          {
      27  SpeedLimiter::SpeedLimiter(  
      28   bool has_velocity_limits,   bool has_acceleration_limits,   bool has_jerk_limits,   double min_velocity,  
           double max_velocity,   double min_acceleration,   double max_acceleration,   double min_jerk,  
           double max_jerk )
          : has_velocity_limits_(  has_velocity_limits ),  
           has_acceleration_limits_(  has_acceleration_limits ),  
           has_jerk_limits_(  has_jerk_limits ),  
           min_velocity_(  min_velocity ),  
           max_velocity_(  max_velocity ),  
           min_acceleration_(  min_acceleration ),  
           max_acceleration_(  max_acceleration ),  
           min_jerk_(  min_jerk ),  
           max_jerk_(  max_jerk )
          {
           // Check if limits are valid,   max must be specified,   min defaults to -max if unspecified
           if (  has_velocity_limits_ )
           {
           if (  std::isnan(  max_velocity_ ) )
           {
           throw std::runtime_error(  "Cannot apply velocity limits if max_velocity is not specified" );
           }
           if (  std::isnan(  min_velocity_ ) )
           {
           min_velocity_ = -max_velocity_;
           }
           }
           if (  has_acceleration_limits_ )
           {
           if (  std::isnan(  max_acceleration_ ) )
           {
           throw std::runtime_error(  
           "Cannot apply acceleration limits if max_acceleration is not specified" );
           }
           if (  std::isnan(  min_acceleration_ ) )
           {
           min_acceleration_ = -max_acceleration_;
           }
           }
           if (  has_jerk_limits_ )
           {
           if (  std::isnan(  max_jerk_ ) )
           {
           throw std::runtime_error(  "Cannot apply jerk limits if max_jerk is not specified" );
           }
           if (  std::isnan(  min_jerk_ ) )
           {
           min_jerk_ = -max_jerk_;
           }
           }
          }
          
      78  double SpeedLimiter::limit(  double & v,   double v0,   double v1,   double dt )
          {
           const double tmp = v;
          
           limit_jerk(  v,   v0,   v1,   dt );
           limit_acceleration(  v,   v0,   dt );
           limit_velocity(  v );
          
           return tmp != 0.0 ? v / tmp : 1.0;
          }
          
      89  double SpeedLimiter::limit_velocity(  double & v )
          {
           const double tmp = v;
          
           if (  has_velocity_limits_ )
           {
           v = rcppmath::clamp(  v,   min_velocity_,   max_velocity_ );
           }
          
           return tmp != 0.0 ? v / tmp : 1.0;
          }
          
     101  double SpeedLimiter::limit_acceleration(  double & v,   double v0,   double dt )
          {
           const double tmp = v;
          
           if (  has_acceleration_limits_ )
           {
           const double dv_min = min_acceleration_ * dt;
           const double dv_max = max_acceleration_ * dt;
          
           const double dv = rcppmath::clamp(  v - v0,   dv_min,   dv_max );
          
           v = v0 + dv;
           }
          
           return tmp != 0.0 ? v / tmp : 1.0;
          }
          
     118  double SpeedLimiter::limit_jerk(  double & v,   double v0,   double v1,   double dt )
          {
           const double tmp = v;
          
           if (  has_jerk_limits_ )
           {
           const double dv = v - v0;
           const double dv0 = v0 - v1;
          
           const double dt2 = 2. * dt * dt;
          
           const double da_min = min_jerk_ * dt2;
           const double da_max = max_jerk_ * dt2;
          
           const double da = rcppmath::clamp(  dv - dv0,   da_min,   da_max );
          
           v = v0 + dv0 + da;
           }
          
           return tmp != 0.0 ? v / tmp : 1.0;
          }
          
          } // namespace diff_drive_controller

ros2_controllers/diff_drive_controller/test/test_diff_drive_controller.cpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          
          #include <array>
          #include <memory>
          #include <string>
          #include <thread>
          #include <utility>
          #include <vector>
          
          #include "diff_drive_controller/diff_drive_controller.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/rclcpp.hpp"
          
          using CallbackReturn = controller_interface::CallbackReturn;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using hardware_interface::LoanedCommandInterface;
          using hardware_interface::LoanedStateInterface;
          using lifecycle_msgs::msg::State;
          using testing::SizeIs;
          
      39  class TestableDiffDriveController : public diff_drive_controller::DiffDriveController
          {
          public:
           using DiffDriveController::DiffDriveController;
           std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist(   )
           {
           std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
           received_velocity_msg_ptr_.get(  ret );
           return ret;
           }
          
           /**
           * @brief wait_for_twist block until a new twist is received.
           * Requires that the executor is not spinned elsewhere between the
           * message publication and the call to this function
           *
           * @return true if new twist msg was received,   false if timeout
           */
           bool wait_for_twist(  
           rclcpp::Executor & executor,  
           const std::chrono::milliseconds & timeout = std::chrono::milliseconds(  500 ) )
           {
           rclcpp::WaitSet wait_set;
      62   wait_set.add_subscription(  velocity_command_subscriber_ );
          
      64   if (  wait_set.wait(  timeout ).kind(   ) == rclcpp::WaitResultKind::Ready )
           {
           executor.spin_some(   );
           return true;
           }
           return false;
           }
          };
          
          class TestDiffDriveController : public ::testing::Test
          {
          protected:
           static void SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
           void SetUp(   ) override
           {
           controller_ = std::make_unique<TestableDiffDriveController>(   );
          
           pub_node = std::make_shared<rclcpp::Node>(  "velocity_publisher" );
           velocity_publisher = pub_node->create_publisher<geometry_msgs::msg::TwistStamped>(  
           controller_name + "/cmd_vel",   rclcpp::SystemDefaultsQoS(   ) );
           }
          
           static void TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
           /// Publish velocity msgs
           /**
           * linear - magnitude of the linear command in the geometry_msgs::twist message
           * angular - the magnitude of the angular command in geometry_msgs::twist message
           */
           void publish(  double linear,   double angular )
           {
           int wait_count = 0;
           auto topic = velocity_publisher->get_topic_name(   );
           while (  pub_node->count_subscribers(  topic ) == 0 )
           {
           if (  wait_count >= 5 )
           {
           auto error_msg = std::string(  "publishing to " ) + topic + " but no node subscribes to it";
           throw std::runtime_error(  error_msg );
           }
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           ++wait_count;
           }
          
           geometry_msgs::msg::TwistStamped velocity_message;
           velocity_message.header.stamp = pub_node->get_clock(   )->now(   );
           velocity_message.twist.linear.x = linear;
           velocity_message.twist.angular.z = angular;
           velocity_publisher->publish(  velocity_message );
           }
          
           /// \brief wait for the subscriber and publisher to completely setup
           void waitForSetup(   )
           {
           constexpr std::chrono::seconds TIMEOUT{2};
           auto clock = pub_node->get_clock(   );
           auto start = clock->now(   );
           while (  velocity_publisher->get_subscription_count(   ) <= 0 )
           {
           if (  (  clock->now(   ) - start ) > TIMEOUT )
           {
           FAIL(   );
           }
           rclcpp::spin_some(  pub_node );
           }
           }
          
           void assignResourcesPosFeedback(   )
           {
           std::vector<LoanedStateInterface> state_ifs;
           state_ifs.emplace_back(  left_wheel_pos_state_ );
           state_ifs.emplace_back(  right_wheel_pos_state_ );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  left_wheel_vel_cmd_ );
           command_ifs.emplace_back(  right_wheel_vel_cmd_ );
          
           controller_->assign_interfaces(  std::move(  command_ifs ),   std::move(  state_ifs ) );
           }
          
           void assignResourcesVelFeedback(   )
           {
           std::vector<LoanedStateInterface> state_ifs;
           state_ifs.emplace_back(  left_wheel_vel_state_ );
           state_ifs.emplace_back(  right_wheel_vel_state_ );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  left_wheel_vel_cmd_ );
           command_ifs.emplace_back(  right_wheel_vel_cmd_ );
          
           controller_->assign_interfaces(  std::move(  command_ifs ),   std::move(  state_ifs ) );
           }
          
           const std::string controller_name = "test_diff_drive_controller";
           std::unique_ptr<TestableDiffDriveController> controller_;
          
           const std::vector<std::string> left_wheel_names = {"left_wheel_joint"};
           const std::vector<std::string> right_wheel_names = {"right_wheel_joint"};
           std::vector<double> position_values_ = {0.1,   0.2};
           std::vector<double> velocity_values_ = {0.01,   0.02};
          
           hardware_interface::StateInterface left_wheel_pos_state_{
           left_wheel_names[0],   HW_IF_POSITION,   &position_values_[0]};
           hardware_interface::StateInterface right_wheel_pos_state_{
           right_wheel_names[0],   HW_IF_POSITION,   &position_values_[1]};
           hardware_interface::StateInterface left_wheel_vel_state_{
           left_wheel_names[0],   HW_IF_VELOCITY,   &velocity_values_[0]};
           hardware_interface::StateInterface right_wheel_vel_state_{
           right_wheel_names[0],   HW_IF_VELOCITY,   &velocity_values_[1]};
           hardware_interface::CommandInterface left_wheel_vel_cmd_{
           left_wheel_names[0],   HW_IF_VELOCITY,   &velocity_values_[0]};
           hardware_interface::CommandInterface right_wheel_vel_cmd_{
           right_wheel_names[0],   HW_IF_VELOCITY,   &velocity_values_[1]};
          
           rclcpp::Node::SharedPtr pub_node;
           rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher;
          };
          
          TEST_F(  TestDiffDriveController,   configure_fails_without_parameters )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   configure_fails_with_only_left_or_only_right_side_defined )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  std::vector<std::string>(   ) ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  std::vector<std::string>(   ) ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   configure_fails_with_mismatching_wheel_side_size )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
          
           auto extended_right_wheel_names = right_wheel_names;
           extended_right_wheel_names.push_back(  "extra_wheel" );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  extended_right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   configure_succeeds_when_wheels_are_specified )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           ASSERT_THAT(  
           controller_->state_interface_configuration(   ).names,  
           SizeIs(  left_wheel_names.size(   ) + right_wheel_names.size(   ) ) );
           ASSERT_THAT(  
           controller_->command_interface_configuration(   ).names,  
           SizeIs(  left_wheel_names.size(   ) + right_wheel_names.size(   ) ) );
          }
          
          TEST_F(  TestDiffDriveController,   activate_fails_without_resources_assigned )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   activate_succeeds_with_pos_resources_assigned )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           // We implicitly test that by default position feedback is required
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           assignResourcesPosFeedback(   );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          }
          
          TEST_F(  TestDiffDriveController,   activate_succeeds_with_vel_resources_assigned )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "position_feedback",   rclcpp::ParameterValue(  false ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           assignResourcesVelFeedback(   );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          }
          
          TEST_F(  TestDiffDriveController,   activate_fails_with_wrong_resources_assigned_1 )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "position_feedback",   rclcpp::ParameterValue(  false ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           assignResourcesPosFeedback(   );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   activate_fails_with_wrong_resources_assigned_2 )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "position_feedback",   rclcpp::ParameterValue(  true ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
          
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           assignResourcesVelFeedback(   );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
          TEST_F(  TestDiffDriveController,   cleanup )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  rclcpp::Parameter(  "wheel_separation",   0.4 ) );
           controller_->get_node(   )->set_parameter(  rclcpp::Parameter(  "wheel_radius",   0.1 ) );
          
           rclcpp::executors::SingleThreadedExecutor executor;
           executor.add_node(  controller_->get_node(   )->get_node_base_interface(   ) );
           auto state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           assignResourcesPosFeedback(   );
          
           state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
          
           waitForSetup(   );
          
           // send msg
           const double linear = 1.0;
           const double angular = 1.0;
           publish(  linear,   angular );
           controller_->wait_for_twist(  executor );
          
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0,   0,   RCL_ROS_TIME ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           state = controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0,   0,   RCL_ROS_TIME ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           state = controller_->get_node(   )->cleanup(   );
           ASSERT_EQ(  State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
          
           // should be stopped
           EXPECT_EQ(  0.0,   left_wheel_vel_cmd_.get_value(   ) );
           EXPECT_EQ(  0.0,   right_wheel_vel_cmd_.get_value(   ) );
          
           executor.cancel(   );
          }
          
          TEST_F(  TestDiffDriveController,   correct_initialization_using_parameters )
          {
           const auto ret = controller_->init(  controller_name );
           ASSERT_EQ(  ret,   controller_interface::return_type::OK );
          
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "left_wheel_names",   rclcpp::ParameterValue(  left_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  
           rclcpp::Parameter(  "right_wheel_names",   rclcpp::ParameterValue(  right_wheel_names ) ) );
           controller_->get_node(   )->set_parameter(  rclcpp::Parameter(  "wheel_separation",   0.4 ) );
           controller_->get_node(   )->set_parameter(  rclcpp::Parameter(  "wheel_radius",   1.0 ) );
          
           rclcpp::executors::SingleThreadedExecutor executor;
           executor.add_node(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           auto state = controller_->get_node(   )->configure(   );
           assignResourcesPosFeedback(   );
          
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           EXPECT_EQ(  0.01,   left_wheel_vel_cmd_.get_value(   ) );
           EXPECT_EQ(  0.02,   right_wheel_vel_cmd_.get_value(   ) );
          
           state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
          
           // send msg
           const double linear = 1.0;
           const double angular = 0.0;
           publish(  linear,   angular );
           // wait for msg is be published to the system
           ASSERT_TRUE(  controller_->wait_for_twist(  executor ) );
          
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0,   0,   RCL_ROS_TIME ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
           EXPECT_EQ(  1.0,   left_wheel_vel_cmd_.get_value(   ) );
           EXPECT_EQ(  1.0,   right_wheel_vel_cmd_.get_value(   ) );
          
           // deactivated
           // wait so controller process the second point when deactivated
           std::this_thread::sleep_for(  std::chrono::milliseconds(  500 ) );
           state = controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0,   0,   RCL_ROS_TIME ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           EXPECT_EQ(  0.0,   left_wheel_vel_cmd_.get_value(   ) ) << "Wheels are halted on deactivate(   )";
           EXPECT_EQ(  0.0,   right_wheel_vel_cmd_.get_value(   ) ) << "Wheels are halted on deactivate(   )";
          
           // cleanup
           state = controller_->get_node(   )->cleanup(   );
           ASSERT_EQ(  State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           EXPECT_EQ(  0.0,   left_wheel_vel_cmd_.get_value(   ) );
           EXPECT_EQ(  0.0,   right_wheel_vel_cmd_.get_value(   ) );
          
           state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           executor.cancel(   );
          }

ros2_controllers/diff_drive_controller/test/test_load_diff_drive_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      22  TEST(  TestLoadDiffDriveController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  ros2_control_test_assets::diffbot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  
           cm.load_controller(  "test_diff_drive_controller",   "diff_drive_controller/DiffDriveController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
          #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
          
          #include <string>
          
          #include "effort_controllers/visibility_control.h"
          #include "forward_command_controller/forward_command_controller.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          namespace effort_controllers
          {
          /**
           * \brief Forward command controller for a set of effort controlled joints (  linear or angular ).
           *
           * This class forwards the commanded efforts down to a set of joints.
           *
           * \param joints Names of the joints to control.
           *
           * Subscribes to:
           * - \b command (  std_msgs::msg::Float64MultiArray ) : The effort commands to apply.
           */
      36  class JointGroupEffortController : public forward_command_controller::ForwardCommandController
          {
          public:
           EFFORT_CONTROLLERS_PUBLIC
      40   JointGroupEffortController(   );
          
           EFFORT_CONTROLLERS_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           EFFORT_CONTROLLERS_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          };
          
          } // namespace effort_controllers
          
          #endif // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_

ros2_controllers/effort_controllers/include/effort_controllers/visibility_control.h

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
          #define EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define EFFORT_CONTROLLERS_EXPORT __attribute__(  (  dllexport ) )
          #define EFFORT_CONTROLLERS_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define EFFORT_CONTROLLERS_EXPORT __declspec(  dllexport )
          #define EFFORT_CONTROLLERS_IMPORT __declspec(  dllimport )
          #endif
          #ifdef EFFORT_CONTROLLERS_BUILDING_DLL
          #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT
          #else
          #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT
          #endif
          #define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC
          #define EFFORT_CONTROLLERS_LOCAL
          #else
          #define EFFORT_CONTROLLERS_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define EFFORT_CONTROLLERS_IMPORT
          #if __GNUC__ >= 4
          #define EFFORT_CONTROLLERS_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define EFFORT_CONTROLLERS_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define EFFORT_CONTROLLERS_PUBLIC
          #define EFFORT_CONTROLLERS_LOCAL
          #endif
          #define EFFORT_CONTROLLERS_PUBLIC_TYPE
          #endif
          
          #endif // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_

ros2_controllers/effort_controllers/src/joint_group_effort_controller.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "controller_interface/controller_interface.hpp"
          #include "effort_controllers/joint_group_effort_controller.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/parameter.hpp"
          
          namespace effort_controllers
          {
      25  JointGroupEffortController::JointGroupEffortController(   )
          : forward_command_controller::ForwardCommandController(   )
          {
           interface_name_ = hardware_interface::HW_IF_EFFORT;
          }
          
      31  controller_interface::CallbackReturn JointGroupEffortController::on_init(   )
          {
           auto ret = forward_command_controller::ForwardCommandController::on_init(   );
           if (  ret != controller_interface::CallbackReturn::SUCCESS )
           {
           return ret;
           }
          
           try
           {
           // Explicitly set the interface parameter declared by the forward_command_controller
           // to match the value set in the JointGroupEffortController constructor.
           get_node(   )->set_parameter(  
           rclcpp::Parameter(  "interface_name",   hardware_interface::HW_IF_EFFORT ) );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
      55  controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(  
      56   const rclcpp_lifecycle::State & previous_state )
          {
           auto ret = ForwardCommandController::on_deactivate(  previous_state );
          
           // stop all joints
           for (  auto & command_interface : command_interfaces_ )
           {
           command_interface.set_value(  0.0 );
           }
          
           return ret;
          }
          
          } // namespace effort_controllers
          
          #include "pluginlib/class_list_macros.hpp"
          
      73  PLUGINLIB_EXPORT_CLASS(  
           effort_controllers::JointGroupEffortController,   controller_interface::ControllerInterface )

ros2_controllers/effort_controllers/test/test_joint_group_effort_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stddef.h>
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "test_joint_group_effort_controller.hpp"
          
          using CallbackReturn = controller_interface::CallbackReturn;
          using hardware_interface::LoanedCommandInterface;
          
          namespace
          {
      35  rclcpp::WaitResultKind wait_for(  rclcpp::SubscriptionBase::SharedPtr subscription )
          {
           rclcpp::WaitSet wait_set;
           wait_set.add_subscription(  subscription );
           const auto timeout = std::chrono::seconds(  10 );
           return wait_set.wait(  timeout ).kind(   );
          }
          } // namespace
          
      44  void JointGroupEffortControllerTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
      46  void JointGroupEffortControllerTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
      48  void JointGroupEffortControllerTest::SetUp(   )
          {
           controller_ = std::make_unique<FriendJointGroupEffortController>(   );
          }
          
      53  void JointGroupEffortControllerTest::TearDown(   ) { controller_.reset(  nullptr ); }
          
      55  void JointGroupEffortControllerTest::SetUpController(   )
          {
           const auto result = controller_->init(  "test_joint_group_effort_controller" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  joint_1_cmd_ );
           command_ifs.emplace_back(  joint_2_cmd_ );
           command_ifs.emplace_back(  joint_3_cmd_ );
           controller_->assign_interfaces(  std::move(  command_ifs ),   {} );
          }
          
      67  TEST_F(  JointGroupEffortControllerTest,   JointsParameterNotSet )
          {
           SetUpController(   );
          
           // configure failed,   'joints' parameter not set
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      75  TEST_F(  JointGroupEffortControllerTest,   JointsParameterIsEmpty )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>(   )} );
          
           // configure failed,   'joints' is empty
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      84  TEST_F(  JointGroupEffortControllerTest,   ConfigureAndActivateParamsSuccess )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // configure successful
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          }
          
      94  TEST_F(  JointGroupEffortControllerTest,   ActivateWithWrongJointsNamesFails )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint4"}} );
          
           // activate failed,   'joint4' is not a valid joint name for the hardware
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint2"}} );
          
           // activate failed,   'acceleration' is not a registered interface for `joint1`
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
     110  TEST_F(  JointGroupEffortControllerTest,   CommandSuccessTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful though no command has been send yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands have been modified
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   30.0 );
          }
          
     142  TEST_F(  JointGroupEffortControllerTest,   WrongCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // send command with wrong number of joints
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update failed,   command size does not match number of joints
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          }
          
     164  TEST_F(  JointGroupEffortControllerTest,   NoCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful,   no command received yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          }
          
     181  TEST_F(  JointGroupEffortControllerTest,   CommandCallbackTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // default values
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           auto node_state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // send a new command
           rclcpp::Node test_node(  "test_node" );
           auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(  
           std::string(  controller_->get_node(   )->get_name(   ) ) + "/commands",   rclcpp::SystemDefaultsQoS(   ) );
           std_msgs::msg::Float64MultiArray command_msg;
           command_msg.data = {10.0,   20.0,   30.0};
           command_pub->publish(  command_msg );
          
           // wait for command message to be passed
           ASSERT_EQ(  wait_for(  controller_->joints_command_subscriber_ ),   rclcpp::WaitResultKind::Ready );
          
           // process callbacks
           rclcpp::spin_some(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   30.0 );
          }
          
     222  TEST_F(  JointGroupEffortControllerTest,   StopJointsOnDeactivateTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // configure successful
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           // stop the controller
           ASSERT_EQ(  controller_->on_deactivate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // check joint commands are now zero
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   0.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   0.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   0.0 );
          }

ros2_controllers/effort_controllers/test/test_joint_group_effort_controller.hpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
          #define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "effort_controllers/joint_group_effort_controller.hpp"
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::HW_IF_EFFORT;
          // subclassing and friending so we can access member variables
      31  class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController
          {
      33   FRIEND_TEST(  JointGroupEffortControllerTest,   CommandSuccessTest );
      34   FRIEND_TEST(  JointGroupEffortControllerTest,   WrongCommandCheckTest );
      35   FRIEND_TEST(  JointGroupEffortControllerTest,   CommandCallbackTest );
      36   FRIEND_TEST(  JointGroupEffortControllerTest,   StopJointsOnDeactivateTest );
          };
          
      39  class JointGroupEffortControllerTest : public ::testing::Test
          {
          public:
      42   static void SetUpTestCase(   );
      43   static void TearDownTestCase(   );
          
      45   void SetUp(   );
      46   void TearDown(   );
          
      48   void SetUpController(   );
          
          protected:
      51   std::unique_ptr<FriendJointGroupEffortController> controller_;
          
           // dummy joint state values used for tests
      54   const std::vector<std::string> joint_names_ = {"joint1",   "joint2",   "joint3"};
      55   std::vector<double> joint_commands_ = {1.1,   2.1,   3.1};
          
           CommandInterface joint_1_cmd_{joint_names_[0],   HW_IF_EFFORT,   &joint_commands_[0]};
      58   CommandInterface joint_2_cmd_{joint_names_[1],   HW_IF_EFFORT,   &joint_commands_[1]};
           CommandInterface joint_3_cmd_{joint_names_[2],   HW_IF_EFFORT,   &joint_commands_[2]};
          };
          
          #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_

ros2_controllers/effort_controllers/test/test_load_joint_group_effort_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      23  TEST(  TestLoadJointGroupVelocityController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_joint_group_effort_controller",   "effort_controllers/JointGroupEffortController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #ifndef FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
          #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "force_torque_sensor_broadcaster/visibility_control.h"
          #include "geometry_msgs/msg/wrench_stamped.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "realtime_tools/realtime_publisher.h"
          #include "semantic_components/force_torque_sensor.hpp"
          
          namespace force_torque_sensor_broadcaster
          {
      36  class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface
          {
          public:
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
      40   ForceTorqueSensorBroadcaster(   );
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init(   ) override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
          protected:
           std::string sensor_name_;
           std::array<std::string,   6> interface_names_;
           std::string frame_id_;
          
           std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
          
           using StatePublisher = realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>;
           rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr sensor_state_publisher_;
           std::unique_ptr<StatePublisher> realtime_publisher_;
          };
          
          } // namespace force_torque_sensor_broadcaster
          
          #endif // FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_

ros2_controllers/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/visibility_control.h

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Subhas Das,   Denis Stogl
           */
          
          #ifndef FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
          #define FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__(  (  dllexport ) )
          #define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __declspec(  dllexport )
          #define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT
          #else
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT
          #endif
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
          #define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL
          #else
          #define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT
          #if __GNUC__ >= 4
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
          #define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL
          #endif
          #define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE
          #endif
          
          #endif // FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_

ros2_controllers/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
          
          #include <memory>
          #include <string>
          
          namespace force_torque_sensor_broadcaster
          {
      26  ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster(   )
          : controller_interface::ControllerInterface(   )
          {
          }
          
      31  controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init(   )
          {
           try
           {
           auto_declare<std::string>(  "sensor_name",   "" );
           auto_declare<std::string>(  "interface_names.force.x",   "" );
           auto_declare<std::string>(  "interface_names.force.y",   "" );
           auto_declare<std::string>(  "interface_names.force.z",   "" );
           auto_declare<std::string>(  "interface_names.torque.x",   "" );
           auto_declare<std::string>(  "interface_names.torque.y",   "" );
           auto_declare<std::string>(  "interface_names.torque.z",   "" );
           auto_declare<std::string>(  "frame_id",   "" );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
      53  controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(  
      54   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           sensor_name_ = get_node(   )->get_parameter(  "sensor_name" ).as_string(   );
           interface_names_[0] = get_node(   )->get_parameter(  "interface_names.force.x" ).as_string(   );
           interface_names_[1] = get_node(   )->get_parameter(  "interface_names.force.y" ).as_string(   );
           interface_names_[2] = get_node(   )->get_parameter(  "interface_names.force.z" ).as_string(   );
           interface_names_[3] = get_node(   )->get_parameter(  "interface_names.torque.x" ).as_string(   );
           interface_names_[4] = get_node(   )->get_parameter(  "interface_names.torque.y" ).as_string(   );
           interface_names_[5] = get_node(   )->get_parameter(  "interface_names.torque.z" ).as_string(   );
          
           const bool no_interface_names_defined =
           std::count(  interface_names_.begin(   ),   interface_names_.end(   ),   "" ) == 6;
          
           if (  sensor_name_.empty(   ) && no_interface_names_defined )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "'sensor_name' or at least one "
           "'interface_names.[force|torque].[x|y|z]' parameter has to be specified." );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           if (  !sensor_name_.empty(   ) && !no_interface_names_defined )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "both 'sensor_name' and "
           "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together." );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           frame_id_ = get_node(   )->get_parameter(  "frame_id" ).as_string(   );
           if (  frame_id_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'frame_id' parameter has to be provided." );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           if (  !sensor_name_.empty(   ) )
           {
           force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(  
           semantic_components::ForceTorqueSensor(  sensor_name_ ) );
           }
           else
           {
           force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(  
           semantic_components::ForceTorqueSensor(  
           interface_names_[0],   interface_names_[1],   interface_names_[2],   interface_names_[3],  
           interface_names_[4],   interface_names_[5] ) );
           }
          
           try
           {
           // register ft sensor data publisher
           sensor_state_publisher_ = get_node(   )->create_publisher<geometry_msgs::msg::WrenchStamped>(  
           "~/wrench",   rclcpp::SystemDefaultsQoS(   ) );
           realtime_publisher_ = std::make_unique<StatePublisher>(  sensor_state_publisher_ );
           }
           catch (  const std::exception & e )
           {
           fprintf(  
           stderr,   "Exception thrown during publisher creation at configure stage with message : %s \n",  
           e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           realtime_publisher_->lock(   );
           realtime_publisher_->msg_.header.frame_id = frame_id_;
           realtime_publisher_->unlock(   );
          
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "configure successful" );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::InterfaceConfiguration
     129  ForceTorqueSensorBroadcaster::command_interface_configuration(   ) const
          {
           controller_interface::InterfaceConfiguration command_interfaces_config;
           command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
           return command_interfaces_config;
          }
          
          controller_interface::InterfaceConfiguration
     137  ForceTorqueSensorBroadcaster::state_interface_configuration(   ) const
          {
           controller_interface::InterfaceConfiguration state_interfaces_config;
           state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           state_interfaces_config.names = force_torque_sensor_->get_state_interface_names(   );
           return state_interfaces_config;
          }
          
     145  controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_activate(  
     146   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           force_torque_sensor_->assign_loaned_state_interfaces(  state_interfaces_ );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
     152  controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate(  
     153   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           force_torque_sensor_->release_interfaces(   );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
     159  controller_interface::return_type ForceTorqueSensorBroadcaster::update(  
     160   const rclcpp::Time & time,   const rclcpp::Duration & /*period*/ )
          {
           if (  realtime_publisher_ && realtime_publisher_->trylock(   ) )
           {
           realtime_publisher_->msg_.header.stamp = time;
           force_torque_sensor_->get_values_as_message(  realtime_publisher_->msg_.wrench );
           realtime_publisher_->unlockAndPublish(   );
           }
          
           return controller_interface::return_type::OK;
          }
          
          } // namespace force_torque_sensor_broadcaster
          
          #include "pluginlib/class_list_macros.hpp"
          
     176  PLUGINLIB_EXPORT_CLASS(  
           force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,  
           controller_interface::ControllerInterface )

ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

          // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #include "test_force_torque_sensor_broadcaster.hpp"
          
          #include <memory>
          #include <utility>
          #include <vector>
          
          #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
          #include "geometry_msgs/msg/wrench_stamped.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          using hardware_interface::LoanedStateInterface;
          
          namespace
          {
      38  constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
          constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
          } // namespace
          
          void ForceTorqueSensorBroadcasterTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
          void ForceTorqueSensorBroadcasterTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
          void ForceTorqueSensorBroadcasterTest::SetUp(   )
          {
           // initialize controller
           fts_broadcaster_ = std::make_unique<FriendForceTorqueSensorBroadcaster>(   );
          }
          
          void ForceTorqueSensorBroadcasterTest::TearDown(   ) { fts_broadcaster_.reset(  nullptr ); }
          
          void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster(   )
          {
           const auto result = fts_broadcaster_->init(  "test_force_torque_sensor_broadcaster" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedStateInterface> state_ifs;
           state_ifs.emplace_back(  fts_force_x_ );
           state_ifs.emplace_back(  fts_force_y_ );
           state_ifs.emplace_back(  fts_force_z_ );
           state_ifs.emplace_back(  fts_torque_x_ );
           state_ifs.emplace_back(  fts_torque_y_ );
           state_ifs.emplace_back(  fts_torque_z_ );
          
           fts_broadcaster_->assign_interfaces(  {},   std::move(  state_ifs ) );
          }
          
          void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(  
           geometry_msgs::msg::WrenchStamped & wrench_msg )
          {
           // create a new subscriber
           rclcpp::Node test_subscription_node(  "test_subscription_node" );
           auto subs_callback = [&](  const geometry_msgs::msg::WrenchStamped::SharedPtr ) {};
           auto subscription = test_subscription_node.create_subscription<geometry_msgs::msg::WrenchStamped>(  
           "/test_force_torque_sensor_broadcaster/wrench",   10,   subs_callback );
          
           // call update to publish the test value
           // since update doesn't guarantee a published message,   republish until received
           int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
           rclcpp::WaitSet wait_set; // block used to wait on message
           wait_set.add_subscription(  subscription );
           while (  max_sub_check_loop_count-- )
           {
           fts_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // check if message has been received
           if (  wait_set.wait(  std::chrono::milliseconds(  2 ) ).kind(   ) == rclcpp::WaitResultKind::Ready )
           {
           break;
           }
           }
           ASSERT_GE(  max_sub_check_loop_count,   0 ) << "Test was unable to publish a message through "
           "controller/broadcaster update loop";
          
           // take message from subscription
           rclcpp::MessageInfo msg_info;
           ASSERT_TRUE(  subscription->take(  wrench_msg,   msg_info ) );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_InterfaceNames_NotSet )
          {
           SetUpFTSBroadcaster(   );
          
           // configure failed
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_FrameId_NotSet )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'interface_names'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
          
           // configure failed,   'frame_id' parameter not set
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   InterfaceNames_FrameId_NotSet )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'sensor_name'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
          
           // configure failed,   'frame_id' parameter not set
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_InterfaceNames_Set )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'sensor_name'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
          
           // set the 'interface_names'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
          
           // configure failed,   both 'sensor_name' and 'interface_names' supplied
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_IsEmpty_InterfaceNames_NotSet )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'sensor_name' empty
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   ""} );
          
           // configure failed,   'sensor_name' parameter empty
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   InterfaceNames_IsEmpty_SensorName_NotSet )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'interface_names' empty
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   ""} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   ""} );
          
           // configure failed,   'interface_name' parameter empty
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_Configure_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'sensor_name'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
          
           // set the 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           // configure passed
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   InterfaceNames_Configure_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the 'interface_names'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
          
           // set the 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           // configure passed
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_Activate_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           // configure and activate success
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_Update_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  
           fts_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   InterfaceNames_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the params 'interface_names' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  
           fts_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   SensorName_Publish_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           geometry_msgs::msg::WrenchStamped wrench_msg;
           subscribe_and_get_message(  wrench_msg );
          
           ASSERT_EQ(  wrench_msg.header.frame_id,   frame_id_ );
           ASSERT_EQ(  wrench_msg.wrench.force.x,   sensor_values_[0] );
           ASSERT_EQ(  wrench_msg.wrench.force.y,   sensor_values_[1] );
           ASSERT_EQ(  wrench_msg.wrench.force.z,   sensor_values_[2] );
           ASSERT_EQ(  wrench_msg.wrench.torque.x,   sensor_values_[3] );
           ASSERT_EQ(  wrench_msg.wrench.torque.y,   sensor_values_[4] );
           ASSERT_EQ(  wrench_msg.wrench.torque.z,   sensor_values_[5] );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   InterfaceNames_Publish_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set the params 'interface_names' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           geometry_msgs::msg::WrenchStamped wrench_msg;
           subscribe_and_get_message(  wrench_msg );
          
           ASSERT_EQ(  wrench_msg.header.frame_id,   frame_id_ );
           ASSERT_EQ(  wrench_msg.wrench.force.x,   sensor_values_[0] );
           ASSERT_TRUE(  std::isnan(  wrench_msg.wrench.force.y ) );
           ASSERT_TRUE(  std::isnan(  wrench_msg.wrench.force.z ) );
           ASSERT_TRUE(  std::isnan(  wrench_msg.wrench.torque.x ) );
           ASSERT_TRUE(  std::isnan(  wrench_msg.wrench.torque.y ) );
           ASSERT_EQ(  wrench_msg.wrench.torque.z,   sensor_values_[5] );
          }
          
          TEST_F(  ForceTorqueSensorBroadcasterTest,   All_InterfaceNames_Publish_Success )
          {
           SetUpFTSBroadcaster(   );
          
           // set all the params 'interface_names' and 'frame_id'
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.x",   "fts_sensor/force.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.y",   "fts_sensor/force.y"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.force.z",   "fts_sensor/force.z"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.x",   "fts_sensor/torque.x"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.y",   "fts_sensor/torque.y"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"interface_names.torque.z",   "fts_sensor/torque.z"} );
           fts_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  fts_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  fts_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           geometry_msgs::msg::WrenchStamped wrench_msg;
           subscribe_and_get_message(  wrench_msg );
          
           ASSERT_EQ(  wrench_msg.header.frame_id,   frame_id_ );
           ASSERT_EQ(  wrench_msg.wrench.force.x,   sensor_values_[0] );
           ASSERT_EQ(  wrench_msg.wrench.force.y,   sensor_values_[1] );
           ASSERT_EQ(  wrench_msg.wrench.force.z,   sensor_values_[2] );
           ASSERT_EQ(  wrench_msg.wrench.torque.x,   sensor_values_[3] );
           ASSERT_EQ(  wrench_msg.wrench.torque.y,   sensor_values_[4] );
           ASSERT_EQ(  wrench_msg.wrench.torque.z,   sensor_values_[5] );
          }

ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp

          // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl
           */
          
          #ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
          #define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "gmock/gmock.h"
          
          #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
          
          // subclassing and friending so we can access member variables
      30  class FriendForceTorqueSensorBroadcaster
      31  : public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster
          {
      33   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   SensorNameParameterNotSet );
      34   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   InterfaceNamesParameterNotSet );
      35   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   FrameIdParameterNotSet );
      36   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   SensorNameParameterIsEmpty );
      37   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   InterfaceNameParameterIsEmpty );
          
      39   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   ActivateSuccess );
      40   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   UpdateTest );
      41   FRIEND_TEST(  ForceTorqueSensorBroadcasterTest,   SensorStatePublishTest );
          };
          
      44  class ForceTorqueSensorBroadcasterTest : public ::testing::Test
          {
          public:
      47   static void SetUpTestCase(   );
      48   static void TearDownTestCase(   );
          
      50   void SetUp(   );
      51   void TearDown(   );
          
      53   void SetUpFTSBroadcaster(   );
          
          protected:
      56   const std::string sensor_name_ = "fts_sensor";
      57   const std::string frame_id_ = "fts_sensor_frame";
      58   std::array<double,   6> sensor_values_ = {1.1,   2.2,   3.3,   4.4,   5.5,   6.6};
          
           hardware_interface::StateInterface fts_force_x_{sensor_name_,   "force.x",   &sensor_values_[0]};
      61   hardware_interface::StateInterface fts_force_y_{sensor_name_,   "force.y",   &sensor_values_[1]};
           hardware_interface::StateInterface fts_force_z_{sensor_name_,   "force.z",   &sensor_values_[2]};
           hardware_interface::StateInterface fts_torque_x_{sensor_name_,   "torque.x",   &sensor_values_[3]};
           hardware_interface::StateInterface fts_torque_y_{sensor_name_,   "torque.y",   &sensor_values_[4]};
           hardware_interface::StateInterface fts_torque_z_{sensor_name_,   "torque.z",   &sensor_values_[5]};
          
           std::unique_ptr<FriendForceTorqueSensorBroadcaster> fts_broadcaster_;
          
           void subscribe_and_get_message(  geometry_msgs::msg::WrenchStamped & wrench_msg );
          };
          
          #endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_

ros2_controllers/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Subhas Das,   Denis Stogl
           */
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      29  TEST(  TestLoadForceTorqueSensorBroadcaster,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_force_torque_sensor_broadcaster",  
           "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_
          #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_
          
          #include <string>
          #include <vector>
          
          #include "forward_command_controller/forward_controllers_base.hpp"
          #include "forward_command_controller/visibility_control.h"
          
          namespace forward_command_controller
          {
          /**
           * \brief Forward command controller for a set of joints.
           *
           * This class forwards the command signal down to a set of joints on the specified interface.
           *
           * \param joints Names of the joints to control.
           * \param interface_name Name of the interface to command.
           *
           * Subscribes to:
           * - \b commands (  std_msgs::msg::Float64MultiArray ) : The commands to apply.
           */
      37  class ForwardCommandController : public ForwardControllersBase
          {
          public:
           FORWARD_COMMAND_CONTROLLER_PUBLIC
      41   ForwardCommandController(   );
          
          protected:
           void declare_parameters(   ) override;
           controller_interface::CallbackReturn read_parameters(   ) override;
          
           std::vector<std::string> joint_names_;
           std::string interface_name_;
          };
          
          } // namespace forward_command_controller
          
          #endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_

ros2_controllers/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp

       1  // Copyright 2021 Stogl Robotics Consulting UG (  haftungsbescrhänkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_
          #define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "forward_command_controller/visibility_control.h"
          #include "rclcpp/subscription.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "realtime_tools/realtime_buffer.h"
          #include "std_msgs/msg/float64_multi_array.hpp"
          
          namespace forward_command_controller
          {
          using CmdType = std_msgs::msg::Float64MultiArray;
          
          /**
           * \brief Forward command controller for a set of joints and interfaces.
           *
           * This class forwards the command signal down to a set of joints or interfaces.
           *
           * Subscribes to:
           * - \b commands (  std_msgs::msg::Float64MultiArray ) : The commands to apply.
           */
      42  class ForwardControllersBase : public controller_interface::ControllerInterface
          {
          public:
           FORWARD_COMMAND_CONTROLLER_PUBLIC
      46   ForwardControllersBase(   );
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           ~ForwardControllersBase(   ) = default;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           FORWARD_COMMAND_CONTROLLER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
          protected:
           /**
           * Derived controllers have to declare parameters in this method.
           * Error handling does not have to be done. It is done in `on_init`-method of this class.
           */
           virtual void declare_parameters(   ) = 0;
          
           /**
           * Derived controllers have to read parameters in this method and set `command_interface_types_`
           * variable. The variable is then used to propagate the command interface configuration to
           * controller manager. The method is called from `on_configure`-method of this class.
           *
           * It is expected that error handling of exceptions is done.
           *
           * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are
           * allowed,   controller_interface::CallbackReturn::ERROR otherwise.
           */
           virtual controller_interface::CallbackReturn read_parameters(   ) = 0;
          
           std::vector<std::string> joint_names_;
           std::string interface_name_;
          
           std::vector<std::string> command_interface_types_;
          
           realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
           rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
          };
          
          } // namespace forward_command_controller
          
          #endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_

ros2_controllers/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp

       1  // Copyright 2021 Stogl Robotics Consulting UG (  haftungsbescrhänkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
          #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
          
          #include <string>
          #include <vector>
          
          #include "forward_command_controller/forward_controllers_base.hpp"
          #include "forward_command_controller/visibility_control.h"
          
          namespace forward_command_controller
          {
          /**
           * \brief Multi interface forward command controller for a set of interfaces.
           *
           * This class forwards the command signal down to a set of interfaces on the specified joint.
           *
           * \param joint Name of the joint to control.
           * \param interface_names Names of the interfaces to command.
           *
           * Subscribes to:
           * - \b commands (  std_msgs::msg::Float64MultiArray ) : The commands to apply.
           */
      37  class MultiInterfaceForwardCommandController
      38  : public forward_command_controller::ForwardControllersBase
          {
          public:
           FORWARD_COMMAND_CONTROLLER_PUBLIC
      42   MultiInterfaceForwardCommandController(   );
          
          protected:
           void declare_parameters(   ) override;
           controller_interface::CallbackReturn read_parameters(   ) override;
          
           std::string joint_name_;
           std::vector<std::string> interface_names_;
          };
          
          } // namespace forward_command_controller
          
          #endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_

ros2_controllers/forward_command_controller/include/forward_command_controller/visibility_control.h

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_
          #define FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__(  (  dllexport ) )
          #define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec(  dllexport )
          #define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT
          #else
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT
          #endif
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC
          #define FORWARD_COMMAND_CONTROLLER_LOCAL
          #else
          #define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define FORWARD_COMMAND_CONTROLLER_IMPORT
          #if __GNUC__ >= 4
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC
          #define FORWARD_COMMAND_CONTROLLER_LOCAL
          #endif
          #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE
          #endif
          
          #endif // FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_

ros2_controllers/forward_command_controller/src/forward_command_controller.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "forward_command_controller/forward_command_controller.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "rclcpp/logging.hpp"
          #include "rclcpp/qos.hpp"
          
          namespace forward_command_controller
          {
      28  ForwardCommandController::ForwardCommandController(   ) : ForwardControllersBase(   ) {}
          
      30  void ForwardCommandController::declare_parameters(   )
          {
           auto_declare(  "joints",   std::vector<std::string>(   ) );
           auto_declare(  "interface_name",   std::string(   ) );
          }
          
      36  controller_interface::CallbackReturn ForwardCommandController::read_parameters(   )
          {
           joint_names_ = get_node(   )->get_parameter(  "joints" ).as_string_array(   );
          
           if (  joint_names_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'joints' parameter was empty" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           // Specialized,   child controllers set interfaces before calling configure function.
           if (  interface_name_.empty(   ) )
           {
           interface_name_ = get_node(   )->get_parameter(  "interface_name" ).as_string(   );
           }
          
           if (  interface_name_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'interface_name' parameter was empty" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           for (  const auto & joint : joint_names_ )
           {
           command_interface_types_.push_back(  joint + "/" + interface_name_ );
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          } // namespace forward_command_controller
          
          #include "pluginlib/class_list_macros.hpp"
          
      70  PLUGINLIB_EXPORT_CLASS(  
           forward_command_controller::ForwardCommandController,   controller_interface::ControllerInterface )

ros2_controllers/forward_command_controller/src/forward_controllers_base.cpp

       1  // Copyright 2021 Stogl Robotics Consulting UG (  haftungsbescrhänkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "forward_command_controller/forward_controllers_base.hpp"
          
          #include <algorithm>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "controller_interface/helpers.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/qos.hpp"
          
          namespace forward_command_controller
          {
      30  ForwardControllersBase::ForwardControllersBase(   )
          : controller_interface::ControllerInterface(   ),  
           rt_command_ptr_(  nullptr ),  
           joints_command_subscriber_(  nullptr )
          {
          }
          
      37  controller_interface::CallbackReturn ForwardControllersBase::on_init(   )
          {
           try
           {
           declare_parameters(   );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
      52  controller_interface::CallbackReturn ForwardControllersBase::on_configure(  
      53   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           auto ret = this->read_parameters(   );
           if (  ret != controller_interface::CallbackReturn::SUCCESS )
           {
           return ret;
           }
          
           joints_command_subscriber_ = get_node(   )->create_subscription<CmdType>(  
           "~/commands",   rclcpp::SystemDefaultsQoS(   ),  
           [this](  const CmdType::SharedPtr msg ) { rt_command_ptr_.writeFromNonRT(  msg ); } );
          
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "configure successful" );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          controller_interface::InterfaceConfiguration
      70  ForwardControllersBase::command_interface_configuration(   ) const
          {
           controller_interface::InterfaceConfiguration command_interfaces_config;
           command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           command_interfaces_config.names = command_interface_types_;
          
           return command_interfaces_config;
          }
          
      79  controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration(   )
           const
          {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
          }
          
      86  controller_interface::CallbackReturn ForwardControllersBase::on_activate(  
      87   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           // check if we have all resources defined in the "points" parameter
           // also verify that we *only* have the resources defined in the "points" parameter
           // ATTENTION(  destogl ): Shouldn't we use ordered interface all the time?
           std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           ordered_interfaces;
           if (  
           !controller_interface::get_ordered_interfaces(  
           command_interfaces_,   command_interface_types_,   std::string(  "" ),   ordered_interfaces ) ||
           command_interface_types_.size(   ) != ordered_interfaces.size(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Expected %zu command interfaces,   got %zu",  
           command_interface_types_.size(   ),   ordered_interfaces.size(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           // reset command buffer if a command came through callback when controller was inactive
           rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(  nullptr );
          
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "activate successful" );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
     112  controller_interface::CallbackReturn ForwardControllersBase::on_deactivate(  
     113   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           // reset command buffer
           rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(  nullptr );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
     120  controller_interface::return_type ForwardControllersBase::update(  
     121   const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           auto joint_commands = rt_command_ptr_.readFromRT(   );
          
           // no command received yet
           if (  !joint_commands || !(  *joint_commands ) )
           {
           return controller_interface::return_type::OK;
           }
          
           if (  (  *joint_commands )->data.size(   ) != command_interfaces_.size(   ) )
           {
           RCLCPP_ERROR_THROTTLE(  
           get_node(   )->get_logger(   ),   *(  get_node(   )->get_clock(   ) ),   1000,  
           "command size (  %zu ) does not match number of interfaces (  %zu )",  
           (  *joint_commands )->data.size(   ),   command_interfaces_.size(   ) );
           return controller_interface::return_type::ERROR;
           }
          
           for (  auto index = 0ul; index < command_interfaces_.size(   ); ++index )
           {
           command_interfaces_[index].set_value(  (  *joint_commands )->data[index] );
           }
          
           return controller_interface::return_type::OK;
          }
          
          } // namespace forward_command_controller

ros2_controllers/forward_command_controller/src/multi_interface_forward_command_controller.cpp

       1  // Copyright 2021 Stogl Robotics Consulting UG (  haftungsbescrhänkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "forward_command_controller/multi_interface_forward_command_controller.hpp"
          
          #include <string>
          #include <vector>
          
          namespace forward_command_controller
          {
      22  MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController(   )
          : ForwardControllersBase(   )
          {
          }
          
      27  void MultiInterfaceForwardCommandController::declare_parameters(   )
          {
           auto_declare(  "joint",   joint_name_ );
           auto_declare(  "interface_names",   interface_names_ );
          }
          
      33  controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters(   )
          {
           joint_name_ = get_node(   )->get_parameter(  "joint" ).as_string(   );
           interface_names_ = get_node(   )->get_parameter(  "interface_names" ).as_string_array(   );
          
           if (  joint_name_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'joint' parameter is empty" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           if (  interface_names_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'interfaces' parameter is empty" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           for (  const auto & interface : interface_names_ )
           {
           command_interface_types_.push_back(  joint_name_ + "/" + interface );
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          } // namespace forward_command_controller
          
          #include "pluginlib/class_list_macros.hpp"
          
      62  PLUGINLIB_EXPORT_CLASS(  
           forward_command_controller::MultiInterfaceForwardCommandController,  
           controller_interface::ControllerInterface )

ros2_controllers/forward_command_controller/test/test_forward_command_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "test_forward_command_controller.hpp"
          
          #include "forward_command_controller/forward_command_controller.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/node.hpp"
          #include "rclcpp/qos.hpp"
          #include "rclcpp/subscription.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp/wait_set.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          using hardware_interface::LoanedCommandInterface;
          
          namespace
          {
      40  rclcpp::WaitResultKind wait_for(  rclcpp::SubscriptionBase::SharedPtr subscription )
          {
           rclcpp::WaitSet wait_set;
           wait_set.add_subscription(  subscription );
           const auto timeout = std::chrono::seconds(  10 );
           return wait_set.wait(  timeout ).kind(   );
          }
          } // namespace
          
      49  void ForwardCommandControllerTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
      51  void ForwardCommandControllerTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
      53  void ForwardCommandControllerTest::SetUp(   )
          {
           // initialize controller
           controller_ = std::make_unique<FriendForwardCommandController>(   );
          }
          
      59  void ForwardCommandControllerTest::TearDown(   ) { controller_.reset(  nullptr ); }
          
      61  void ForwardCommandControllerTest::SetUpController(   )
          {
           const auto result = controller_->init(  "forward_command_controller" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  joint_1_pos_cmd_ );
           command_ifs.emplace_back(  joint_2_pos_cmd_ );
           command_ifs.emplace_back(  joint_3_pos_cmd_ );
           controller_->assign_interfaces(  std::move(  command_ifs ),   {} );
          }
          
      73  TEST_F(  ForwardCommandControllerTest,   JointsParameterNotSet )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"interface_name",   ""} );
          
           // configure failed,   'joints' parameter not set
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
      84  TEST_F(  ForwardCommandControllerTest,   InterfaceParameterNotSet )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>(   )} );
          
           // configure failed,   'interface_name' parameter not set
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
      95  TEST_F(  ForwardCommandControllerTest,   JointsParameterIsEmpty )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>(   )} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   ""} );
          
           // configure failed,   'joints' is empty
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     108  TEST_F(  ForwardCommandControllerTest,   InterfaceParameterEmpty )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint2"}} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   ""} );
          
           // configure failed,   'interface_name' is empty
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     120  TEST_F(  ForwardCommandControllerTest,   ConfigureParamsSuccess )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint2"}} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           // configure successful
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          }
          
     133  TEST_F(  ForwardCommandControllerTest,   ActivateWithWrongJointsNamesFails )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint4"}} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           // activate failed,   'joint4' is not a valid joint name for the hardware
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
           ASSERT_EQ(  
           controller_->on_activate(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     149  TEST_F(  ForwardCommandControllerTest,   ActivateWithWrongInterfaceNameFails )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "acceleration"} );
          
           // activate failed,   'acceleration' is not a registered interface for `joint1`
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
           ASSERT_EQ(  
           controller_->on_activate(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     165  TEST_F(  ForwardCommandControllerTest,   ActivateSuccess )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           // activate successful
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
           ASSERT_EQ(  
           controller_->on_activate(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          }
          
     181  TEST_F(  ForwardCommandControllerTest,   CommandSuccessTest )
          {
           SetUpController(   );
          
           // configure controller
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          
           // update successful though no command has been send yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0.1 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands have been modified
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30.0 );
          }
          
     218  TEST_F(  ForwardCommandControllerTest,   WrongCommandCheckTest )
          {
           SetUpController(   );
          
           // configure controller
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          
           // send command with wrong number of joints
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update failed,   command size does not match number of joints
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          }
          
     246  TEST_F(  ForwardCommandControllerTest,   NoCommandCheckTest )
          {
           SetUpController(   );
          
           // configure controller
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          
           // update successful,   no command received yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          }
          
     268  TEST_F(  ForwardCommandControllerTest,   CommandCallbackTest )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           // default values
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          
           auto node_state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // send a new command
           rclcpp::Node test_node(  "test_node" );
           auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(  
           std::string(  controller_->get_node(   )->get_name(   ) ) + "/commands",   rclcpp::SystemDefaultsQoS(   ) );
           std_msgs::msg::Float64MultiArray command_msg;
           command_msg.data = {10.0,   20.0,   30.0};
           command_pub->publish(  command_msg );
          
           // wait for command message to be passed
           ASSERT_EQ(  wait_for(  controller_->joints_command_subscriber_ ),   rclcpp::WaitResultKind::Ready );
          
           // process callbacks
           rclcpp::spin_some(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30.0 );
          }
          
     311  TEST_F(  ForwardCommandControllerTest,   ActivateDeactivateCommandsResetSuccess )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           controller_->get_node(   )->set_parameter(  {"interface_name",   "position"} );
          
           // default values
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          
           auto node_state = controller_->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>(   );
           command_msg->data = {10.0,   20.0,   30.0};
          
           controller_->rt_command_ptr_.writeFromNonRT(  command_msg );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30 );
          
           node_state = controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           // command ptr should be reset (  nullptr ) after deactivation - same check as in `update`
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // Controller is inactive but let's put some data into buffer (  simulate callback when inactive )
           command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>(   );
           command_msg->data = {5.5,   6.6,   7.7};
          
           controller_->rt_command_ptr_.writeFromNonRT(  command_msg );
          
           // command ptr should be available and message should be there - same check as in `update`
           ASSERT_TRUE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_TRUE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // Now activate again
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // command ptr should be reset (  nullptr ) after activation - same check as in `update`
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // values should not change
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30 );
          
           // set commands again
           controller_->rt_command_ptr_.writeFromNonRT(  command_msg );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   5.5 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   6.6 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   7.7 );
          }

ros2_controllers/forward_command_controller/test/test_forward_command_controller.hpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_FORWARD_COMMAND_CONTROLLER_HPP_
          #define TEST_FORWARD_COMMAND_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "forward_command_controller/forward_command_controller.hpp"
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::HW_IF_POSITION;
          
          // subclassing and friending so we can access member variables
      32  class FriendForwardCommandController : public forward_command_controller::ForwardCommandController
          {
      34   FRIEND_TEST(  ForwardCommandControllerTest,   JointsParameterNotSet );
      35   FRIEND_TEST(  ForwardCommandControllerTest,   InterfaceParameterNotSet );
      36   FRIEND_TEST(  ForwardCommandControllerTest,   JointsParameterIsEmpty );
      37   FRIEND_TEST(  ForwardCommandControllerTest,   InterfaceParameterEmpty );
      38   FRIEND_TEST(  ForwardCommandControllerTest,   ConfigureParamsSuccess );
          
      40   FRIEND_TEST(  ForwardCommandControllerTest,   ActivateWithWrongJointsNamesFails );
      41   FRIEND_TEST(  ForwardCommandControllerTest,   ActivateWithWrongInterfaceNameFails );
      42   FRIEND_TEST(  ForwardCommandControllerTest,   ActivateSuccess );
      43   FRIEND_TEST(  ForwardCommandControllerTest,   CommandSuccessTest );
      44   FRIEND_TEST(  ForwardCommandControllerTest,   WrongCommandCheckTest );
      45   FRIEND_TEST(  ForwardCommandControllerTest,   NoCommandCheckTest );
      46   FRIEND_TEST(  ForwardCommandControllerTest,   CommandCallbackTest );
      47   FRIEND_TEST(  ForwardCommandControllerTest,   ActivateDeactivateCommandsResetSuccess );
          };
          
      50  class ForwardCommandControllerTest : public ::testing::Test
          {
          public:
      53   static void SetUpTestCase(   );
      54   static void TearDownTestCase(   );
          
      56   void SetUp(   );
      57   void TearDown(   );
          
      59   void SetUpController(   );
      60   void SetUpHandles(   );
          
          protected:
      63   std::unique_ptr<FriendForwardCommandController> controller_;
          
           // dummy joint state values used for tests
      66   const std::vector<std::string> joint_names_ = {"joint1",   "joint2",   "joint3"};
      67   std::vector<double> joint_commands_ = {1.1,   2.1,   3.1};
          
           CommandInterface joint_1_pos_cmd_{joint_names_[0],   HW_IF_POSITION,   &joint_commands_[0]};
      70   CommandInterface joint_2_pos_cmd_{joint_names_[1],   HW_IF_POSITION,   &joint_commands_[1]};
           CommandInterface joint_3_pos_cmd_{joint_names_[2],   HW_IF_POSITION,   &joint_commands_[2]};
          };
          
          #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_

ros2_controllers/forward_command_controller/test/test_load_forward_command_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      25  TEST(  TestLoadForwardCommandController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_forward_command_controller",   "forward_command_controller/ForwardCommandController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp

       1  // Copyright (  c ) 2021,   PickNik,   Inc.
          // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt ) (  template )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      26  TEST(  TestLoadMultiInterfaceForwardController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_forward_command_controller",  
           "forward_command_controller/MultiInterfaceForwardCommandController" ) );
          }

ros2_controllers/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp

       1  // Copyright (  c ) 2021,   PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          /// \authors: Jack Center,   Denis Stogl
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "test_multi_interface_forward_command_controller.hpp"
          
          #include "forward_command_controller/multi_interface_forward_command_controller.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/node.hpp"
          #include "rclcpp/qos.hpp"
          #include "rclcpp/subscription.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp/wait_set.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          using hardware_interface::LoanedCommandInterface;
          
          namespace
          {
      42  rclcpp::WaitResultKind wait_for(  rclcpp::SubscriptionBase::SharedPtr subscription )
          {
           rclcpp::WaitSet wait_set;
           wait_set.add_subscription(  subscription );
           const auto timeout = std::chrono::seconds(  10 );
           return wait_set.wait(  timeout ).kind(   );
          }
          } // namespace
          
      51  void MultiInterfaceForwardCommandControllerTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
      53  void MultiInterfaceForwardCommandControllerTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
      55  void MultiInterfaceForwardCommandControllerTest::SetUp(   )
          {
           // initialize controller
           controller_ = std::make_unique<FriendMultiInterfaceForwardCommandController>(   );
          }
          
      61  void MultiInterfaceForwardCommandControllerTest::TearDown(   ) { controller_.reset(  nullptr ); }
          
      63  void MultiInterfaceForwardCommandControllerTest::SetUpController(  bool set_params_and_activate )
          {
           const auto result = controller_->init(  "multi_interface_forward_command_controller" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  joint_1_pos_cmd_ );
           command_ifs.emplace_back(  joint_1_vel_cmd_ );
           command_ifs.emplace_back(  joint_1_eff_cmd_ );
           controller_->assign_interfaces(  std::move(  command_ifs ),   {} );
          
           if (  set_params_and_activate )
           {
           SetParametersAndActivateController(   );
           }
          }
          
      80  void MultiInterfaceForwardCommandControllerTest::SetParametersAndActivateController(   )
          {
           controller_->get_node(   )->set_parameter(  {"joint",   "joint1"} );
           controller_->get_node(   )->set_parameter(  
           {"interface_names",   std::vector<std::string>{"position",   "velocity",   "effort"}} );
          
           auto node_state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          }
          
      92  TEST_F(  MultiInterfaceForwardCommandControllerTest,   JointsParameterNotSet )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"interface_names",   std::vector<std::string>(   )} );
          
           // configure failed,   'joint' parameter not set
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     103  TEST_F(  MultiInterfaceForwardCommandControllerTest,   InterfaceParameterNotSet )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joint",   ""} );
          
           // configure failed,   'interface_names' parameter not set
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     114  TEST_F(  MultiInterfaceForwardCommandControllerTest,   JointsParameterIsEmpty )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joint",   ""} );
           controller_->get_node(   )->set_parameter(  {"interface_names",   std::vector<std::string>(   )} );
          
           // configure failed,   'joint' is empty
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     127  TEST_F(  MultiInterfaceForwardCommandControllerTest,   InterfaceParameterEmpty )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joint",   "joint1"} );
           controller_->get_node(   )->set_parameter(  {"interface_names",   std::vector<std::string>(   )} );
          
           // configure failed,   'interface_name' is empty
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     139  TEST_F(  MultiInterfaceForwardCommandControllerTest,   ConfigureParamsSuccess )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joint",   "joint1"} );
           controller_->get_node(   )->set_parameter(  
           {"interface_names",   std::vector<std::string>{"position",   "velocity",   "effort"}} );
          
           // configure successful
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
          }
          
     153  TEST_F(  MultiInterfaceForwardCommandControllerTest,   ActivateWithWrongJointsNamesFails )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joint",   "joint2"} );
           controller_->get_node(   )->set_parameter(  
           {"interface_names",   std::vector<std::string>{"position",   "velocity",   "effort"}} );
          
           // activate failed,   'joint2' is not a valid joint name for the hardware
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
           ASSERT_EQ(  
           controller_->on_activate(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     170  TEST_F(  MultiInterfaceForwardCommandControllerTest,   ActivateWithWrongInterfaceNameFails )
          {
           SetUpController(   );
          
           controller_->get_node(   )->set_parameter(  {"joint",   "joint1"} );
           controller_->get_node(   )->set_parameter(  
           {"interface_names",   std::vector<std::string>{"position",   "velocity",   "acceleration"}} );
          
           // activate failed,   'acceleration' is not a registered interface for `joint1`
           ASSERT_EQ(  
           controller_->on_configure(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::SUCCESS );
           ASSERT_EQ(  
           controller_->on_activate(  rclcpp_lifecycle::State(   ) ),  
           controller_interface::CallbackReturn::ERROR );
          }
          
     187  TEST_F(  MultiInterfaceForwardCommandControllerTest,   ActivateSuccess )
          {
           SetUpController(  true );
          
           // check joint commands are the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   3.1 );
          }
          
     197  TEST_F(  MultiInterfaceForwardCommandControllerTest,   CommandSuccessTest )
          {
           SetUpController(  true );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0.1 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   30.0 );
          }
          
     217  TEST_F(  MultiInterfaceForwardCommandControllerTest,   NoCommandCheckTest )
          {
           SetUpController(  true );
          
           // update successful,   no command received yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   3.1 );
          }
          
     232  TEST_F(  MultiInterfaceForwardCommandControllerTest,   WrongCommandCheckTest )
          {
           SetUpController(  true );
          
           // send command with wrong number of joints
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update failed,   command size does not match number of joints
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   3.1 );
          }
          
     252  TEST_F(  MultiInterfaceForwardCommandControllerTest,   CommandCallbackTest )
          {
           SetUpController(  true );
          
           // send a new command
           rclcpp::Node test_node(  "test_node" );
           auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(  
           std::string(  controller_->get_node(   )->get_name(   ) ) + "/commands",   rclcpp::SystemDefaultsQoS(   ) );
           std_msgs::msg::Float64MultiArray command_msg;
           command_msg.data = {10.0,   20.0,   30.0};
           command_pub->publish(  command_msg );
          
           // wait for command message to be passed
           ASSERT_EQ(  wait_for(  controller_->joints_command_subscriber_ ),   rclcpp::WaitResultKind::Ready );
          
           // process callbacks
           rclcpp::spin_some(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   30.0 );
          }
          
     281  TEST_F(  MultiInterfaceForwardCommandControllerTest,   ActivateDeactivateCommandsResetSuccess )
          {
           SetUpController(  true );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0.1 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   30.0 );
          
           auto node_state = controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           // command ptr should be reset (  nullptr ) after deactivation - same check as in `update`
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // Controller is inactive but let's put some data into buffer (  simulate callback when inactive )
           auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>(   );
           command_msg->data = {5.5,   6.6,   7.7};
           controller_->rt_command_ptr_.writeFromNonRT(  command_msg );
          
           // command ptr should be available and message should be there - same check as in `update`
           ASSERT_TRUE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_TRUE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // Now activate again
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // command ptr should be reset (  nullptr ) after activation - same check as in `update`
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromNonRT(   ) &&
           *(  controller_->rt_command_ptr_.readFromNonRT(   ) ) );
           ASSERT_FALSE(  
           controller_->rt_command_ptr_.readFromRT(   ) && *(  controller_->rt_command_ptr_.readFromRT(   ) ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // values should not change
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   30.0 );
          
           // set commands again
           controller_->rt_command_ptr_.writeFromNonRT(  command_msg );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   5.5 );
           ASSERT_EQ(  joint_1_vel_cmd_.get_value(   ),   6.6 );
           ASSERT_EQ(  joint_1_eff_cmd_.get_value(   ),   7.7 );
          }

ros2_controllers/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp

          // Copyright (  c ) 2021,   PickNik,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          //
          /// \authors: Jack Center,   Denis Stogl
          
          #ifndef TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
          #define TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "forward_command_controller/multi_interface_forward_command_controller.hpp"
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          
          // subclassing and friending so we can access member variables
      36  class FriendMultiInterfaceForwardCommandController
      37  : public forward_command_controller::MultiInterfaceForwardCommandController
          {
      39   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   JointsParameterNotSet );
      40   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   InterfaceParameterNotSet );
      41   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   JointsParameterIsEmpty );
      42   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   InterfaceParameterEmpty );
      43   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   ConfigureParamsSuccess );
          
      45   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   ActivateWithWrongJointsNamesFails );
      46   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   ActivateWithWrongInterfaceNameFails );
      47   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   ActivateSuccess );
      48   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   CommandSuccessTest );
      49   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   WrongCommandCheckTest );
      50   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   NoCommandCheckTest );
      51   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   CommandCallbackTest );
      52   FRIEND_TEST(  MultiInterfaceForwardCommandControllerTest,   ActivateDeactivateCommandsResetSuccess );
          };
          
      55  class MultiInterfaceForwardCommandControllerTest : public ::testing::Test
          {
          public:
      58   static void SetUpTestCase(   );
      59   static void TearDownTestCase(   );
          
      61   void SetUp(   );
      62   void TearDown(   );
          
      64   void SetUpController(  bool set_params_and_activate = false );
      65   void SetParametersAndActivateController(   );
          
          protected:
      68   std::unique_ptr<FriendMultiInterfaceForwardCommandController> controller_;
          
           // dummy joint state value used for tests
      71   const std::string joint_name_ = "joint1";
          
           double pos_cmd_ = 1.1;
           double vel_cmd_ = 2.1;
           double eff_cmd_ = 3.1;
          
           CommandInterface joint_1_pos_cmd_{joint_name_,   HW_IF_POSITION,   &pos_cmd_};
      78   CommandInterface joint_1_vel_cmd_{joint_name_,   HW_IF_VELOCITY,   &vel_cmd_};
           CommandInterface joint_1_eff_cmd_{joint_name_,   HW_IF_EFFORT,   &eff_cmd_};
          };
          
          #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_

ros2_controllers/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

       1  // Copyright 2014,   SRI International
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Sachin Chitta,   Adolfo Rodriguez Tsouroukdissian
          
          #ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_
          #define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_
          
          // C++ standard
          #include <cassert>
          #include <memory>
          #include <stdexcept>
          #include <string>
          // TODO(  JafarAbdi ): Remove experimental once the default standard is C++17
          #include "experimental/optional"
          
          // ROS
          #include "rclcpp/rclcpp.hpp"
          
          // ROS messages
          #include "control_msgs/action/gripper_command.hpp"
          
          // rclcpp_action
          #include "rclcpp_action/create_server.hpp"
          
          // ros_controls
          #include "controller_interface/controller_interface.hpp"
          #include "gripper_controllers/visibility_control.hpp"
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "realtime_tools/realtime_buffer.h"
          #include "realtime_tools/realtime_server_goal_handle.h"
          
          // Project
          #include "gripper_controllers/hardware_interface_adapter.hpp"
          
          namespace gripper_action_controller
          {
          /**
           * \brief Controller for executing a gripper command action for simple
           * single-dof grippers.
           *
           * \tparam HardwareInterface Controller hardware interface. Currently \p
           * hardware_interface::HW_IF_POSITION and \p
           * hardware_interface::HW_IF_EFFORT are supported out-of-the-box.
           */
          template <const char * HardwareInterface>
      59  class GripperActionController : public controller_interface::ControllerInterface
          {
          public:
           /**
           * \brief Store position and max effort in struct to allow easier realtime
           * buffer usage
           */
           struct Commands
           {
           double position_; // Last commanded position
           double max_effort_; // Max allowed effort
           };
          
      72   GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(   );
          
           /**
           * @brief command_interface_configuration This controller requires the
           * position command interfaces for the controlled joints
           */
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           /**
           * @brief command_interface_configuration This controller requires the
           * position and velocity state interfaces for the controlled joints
           */
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           GRIPPER_ACTION_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           realtime_tools::RealtimeBuffer<Commands> command_;
           // pre-allocated memory that is re-used to set the realtime buffer
           Commands command_struct_,   command_struct_rt_;
          
          private:
           using GripperCommandAction = control_msgs::action::GripperCommand;
           using ActionServer = rclcpp_action::Server<GripperCommandAction>;
           using ActionServerPtr = ActionServer::SharedPtr;
           using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
           using RealtimeGoalHandle =
           realtime_tools::RealtimeServerGoalHandle<control_msgs::action::GripperCommand>;
           using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
          
           using HwIfaceAdapter = HardwareInterfaceAdapter<HardwareInterface>;
          
           bool update_hold_position_;
          
           bool verbose_ = false; ///< Hard coded verbose flag to help in debugging
           std::string name_; ///< Controller name.
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           joint_position_command_interface_;
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
           joint_position_state_interface_;
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
           joint_velocity_state_interface_;
          
           std::string joint_name_; ///< Controlled joint names.
          
           HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface.
          
           RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal,   if any.
           control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_;
          
           rclcpp::Duration action_monitor_period_;
          
           // ROS API
           ActionServerPtr action_server_;
          
           rclcpp::TimerBase::SharedPtr goal_handle_timer_;
          
           rclcpp_action::GoalResponse goal_callback(  
           const rclcpp_action::GoalUUID & uuid,   std::shared_ptr<const GripperCommandAction::Goal> goal );
          
           rclcpp_action::CancelResponse cancel_callback(  const std::shared_ptr<GoalHandle> goal_handle );
          
           void accepted_callback(  std::shared_ptr<GoalHandle> goal_handle );
          
           void preempt_active_goal(   );
          
           void set_hold_position(   );
          
           rclcpp::Time last_movement_time_ = rclcpp::Time(  0,   0,   RCL_ROS_TIME ); ///< Store stall time
           double computed_command_; ///< Computed command
          
           double stall_timeout_,  
           stall_velocity_threshold_; ///< Stall related parameters
           double default_max_effort_; ///< Max allowed effort
           double goal_tolerance_;
           bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful
          
           /**
           * \brief Check for success and publish appropriate result and feedback.
           **/
           void check_for_success(  
           const rclcpp::Time & time,   double error_position,   double current_position,  
           double current_velocity );
          };
          
          } // namespace gripper_action_controller
          
          #include "gripper_controllers/gripper_action_controller_impl.hpp"
          
          #endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_

ros2_controllers/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

       1  // Copyright 2014,   SRI International
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Sachin Chitta,   Adolfo Rodriguez Tsouroukdissian,   Stu Glaser
          
          #ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
          #define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
          
          #include "gripper_controllers/gripper_action_controller.hpp"
          
          #include <memory>
          #include <string>
          
          namespace gripper_action_controller
          {
          template <const char * HardwareInterface>
      28  void GripperActionController<HardwareInterface>::preempt_active_goal(   )
          {
           // Cancels the currently active goal
           if (  rt_active_goal_ )
           {
           // Marks the current goal as canceled
           rt_active_goal_->setCanceled(  std::make_shared<GripperCommandAction::Result>(   ) );
           rt_active_goal_.reset(   );
           }
          }
          
          template <const char * HardwareInterface>
      40  controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_init(   )
          {
           try
           {
           // with the lifecycle node being initialized,   we can declare parameters
           auto_declare<double>(  "action_monitor_rate",   20.0 );
           auto_declare<std::string>(  "joint",   joint_name_ );
           auto_declare<double>(  "goal_tolerance",   0.01 );
           auto_declare<double>(  "max_effort",   0.0 );
           auto_declare<bool>(  "allow_stalling",   false );
           auto_declare<double>(  "stall_velocity_threshold",   0.001 );
           auto_declare<double>(  "stall_timeout",   1.0 );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          template <const char * HardwareInterface>
      63  controller_interface::return_type GripperActionController<HardwareInterface>::update(  
      64   const rclcpp::Time & /*time*/,   const rclcpp::Duration & /*period*/ )
          {
           command_struct_rt_ = *(  command_.readFromRT(   ) );
          
           const double current_position = joint_position_state_interface_->get(   ).get_value(   );
           const double current_velocity = joint_velocity_state_interface_->get(   ).get_value(   );
          
           const double error_position = command_struct_rt_.position_ - current_position;
           const double error_velocity = -current_velocity;
          
           check_for_success(  get_node(   )->now(   ),   error_position,   current_position,   current_velocity );
          
           // Hardware interface adapter: Generate and send commands
           computed_command_ = hw_iface_adapter_.updateCommand(  
           command_struct_rt_.position_,   0.0,   error_position,   error_velocity,  
           command_struct_rt_.max_effort_ );
           return controller_interface::return_type::OK;
          }
          
          template <const char * HardwareInterface>
      84  rclcpp_action::GoalResponse GripperActionController<HardwareInterface>::goal_callback(  
      85   const rclcpp_action::GoalUUID &,   std::shared_ptr<const GripperCommandAction::Goal> )
          {
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Received & accepted new action goal" );
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
          }
          
          template <const char * HardwareInterface>
      92  void GripperActionController<HardwareInterface>::accepted_callback(  
      93   std::shared_ptr<GoalHandle> goal_handle ) // Try to update goal
          {
           {
           auto rt_goal = std::make_shared<RealtimeGoalHandle>(  goal_handle );
          
           // Accept new goal
           preempt_active_goal(   );
          
           // This is the non-realtime command_struct
           // We use command_ for sharing
           command_struct_.position_ = goal_handle->get_goal(   )->command.position;
           command_struct_.max_effort_ = goal_handle->get_goal(   )->command.max_effort;
           command_.writeFromNonRT(  command_struct_ );
          
           pre_alloc_result_->reached_goal = false;
           pre_alloc_result_->stalled = false;
          
           last_movement_time_ = get_node(   )->now(   );
           rt_active_goal_ = rt_goal;
           rt_active_goal_->execute(   );
           }
           // Setup goal status checking timer
           goal_handle_timer_ = get_node(   )->create_wall_timer(  
           action_monitor_period_.to_chrono<std::chrono::seconds>(   ),  
           std::bind(  &RealtimeGoalHandle::runNonRealtime,   rt_active_goal_ ) );
          }
          
          template <const char * HardwareInterface>
     121  rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::cancel_callback(  
     122   const std::shared_ptr<GoalHandle> goal_handle )
          {
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Got request to cancel goal" );
          
           // Check that cancel request refers to currently active goal (  if any )
           if (  rt_active_goal_ && rt_active_goal_->gh_ == goal_handle )
           {
           // Enter hold current position mode
           set_hold_position(   );
          
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),   "Canceling active action goal because cancel callback received." );
          
           // Mark the current goal as canceled
           auto action_res = std::make_shared<GripperCommandAction::Result>(   );
           rt_active_goal_->setCanceled(  action_res );
           // Reset current goal
           rt_active_goal_.reset(   );
           }
           return rclcpp_action::CancelResponse::ACCEPT;
          }
          
          template <const char * HardwareInterface>
     145  void GripperActionController<HardwareInterface>::set_hold_position(   )
          {
           command_struct_.position_ = joint_position_state_interface_->get(   ).get_value(   );
           command_struct_.max_effort_ = default_max_effort_;
           command_.writeFromNonRT(  command_struct_ );
          }
          
          template <const char * HardwareInterface>
     153  void GripperActionController<HardwareInterface>::check_for_success(  
     154   const rclcpp::Time & time,   double error_position,   double current_position,  
           double current_velocity )
          {
           if (  !rt_active_goal_ )
           {
           return;
           }
          
           if (  fabs(  error_position ) < goal_tolerance_ )
           {
           pre_alloc_result_->effort = computed_command_;
           pre_alloc_result_->position = current_position;
           pre_alloc_result_->reached_goal = true;
           pre_alloc_result_->stalled = false;
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "Successfully moved to goal." );
           rt_active_goal_->setSucceeded(  pre_alloc_result_ );
           rt_active_goal_.reset(   );
           }
           else
           {
           if (  fabs(  current_velocity ) > stall_velocity_threshold_ )
           {
           last_movement_time_ = time;
           }
           else if (  (  time - last_movement_time_ ).seconds(   ) > stall_timeout_ )
           {
           pre_alloc_result_->effort = computed_command_;
           pre_alloc_result_->position = current_position;
           pre_alloc_result_->reached_goal = false;
           pre_alloc_result_->stalled = true;
           if(  allow_stalling_ )
           {
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "Stall detected moving to goal. Returning success." );
           rt_active_goal_->setSucceeded(  pre_alloc_result_ );
           }
           else
           {
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "Stall detected moving to goal. Aborting action!" );
           rt_active_goal_->setAborted(  pre_alloc_result_ );
           }
           rt_active_goal_.reset(   );
           }
           }
          }
          
          template <const char * HardwareInterface>
     200  controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_configure(  
     201   const rclcpp_lifecycle::State & )
          {
           const auto logger = get_node(   )->get_logger(   );
          
           // Action status checking update rate
           const auto action_monitor_rate = get_node(   )->get_parameter(  "action_monitor_rate" ).as_double(   );
           action_monitor_period_ = rclcpp::Duration::from_seconds(  
           1.0 / get_node(   )->get_parameter(  "action_monitor_rate" ).as_double(   ) );
           RCLCPP_INFO_STREAM(  
           logger,   "Action status changes will be monitored at " << action_monitor_rate << "Hz." );
          
           // Controlled joint
           joint_name_ = get_node(   )->get_parameter(  "joint" ).as_string(   );
           if (  joint_name_.empty(   ) )
           {
           RCLCPP_ERROR(  logger,   "Could not find joint name on param server" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           // Default tolerances
           goal_tolerance_ = get_node(   )->get_parameter(  "goal_tolerance" ).as_double(   );
           goal_tolerance_ = fabs(  goal_tolerance_ );
           // Max allowable effort
           default_max_effort_ = get_node(   )->get_parameter(  "max_effort" ).as_double(   );
           default_max_effort_ = fabs(  default_max_effort_ );
           // Allow stalling will make the action server return success if the
           // gripper stalls when moving to the goal
           allow_stalling_ = get_node(   )->get_parameter(  "allow_stalling" ).as_bool(   );
           // Stall - stall velocity threshold,   stall timeout
           stall_velocity_threshold_ = get_node(   )->get_parameter(  "stall_velocity_threshold" ).as_double(   );
           stall_timeout_ = get_node(   )->get_parameter(  "stall_timeout" ).as_double(   );
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          template <const char * HardwareInterface>
     236  controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_activate(  
     237   const rclcpp_lifecycle::State & )
          {
           auto position_command_interface_it = std::find_if(  
           command_interfaces_.begin(   ),   command_interfaces_.end(   ),  
           [](  const hardware_interface::LoanedCommandInterface & command_interface ) {
           return command_interface.get_interface_name(   ) == hardware_interface::HW_IF_POSITION;
           } );
           if (  position_command_interface_it == command_interfaces_.end(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Expected 1 position command interface" );
           return controller_interface::CallbackReturn::ERROR;
           }
           if (  position_command_interface_it->get_prefix_name(   ) != joint_name_ )
           {
           RCLCPP_ERROR_STREAM(  
           get_node(   )->get_logger(   ),   "Position command interface is different than joint name `"
           << position_command_interface_it->get_prefix_name(   ) << "` != `"
           << joint_name_ << "`" );
           return controller_interface::CallbackReturn::ERROR;
           }
           const auto position_state_interface_it = std::find_if(  
           state_interfaces_.begin(   ),   state_interfaces_.end(   ),  
           [](  const hardware_interface::LoanedStateInterface & state_interface ) {
           return state_interface.get_interface_name(   ) == hardware_interface::HW_IF_POSITION;
           } );
           if (  position_state_interface_it == state_interfaces_.end(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Expected 1 position state interface" );
           return controller_interface::CallbackReturn::ERROR;
           }
           if (  position_state_interface_it->get_prefix_name(   ) != joint_name_ )
           {
           RCLCPP_ERROR_STREAM(  
           get_node(   )->get_logger(   ),   "Position state interface is different than joint name `"
           << position_state_interface_it->get_prefix_name(   ) << "` != `"
           << joint_name_ << "`" );
           return controller_interface::CallbackReturn::ERROR;
           }
           const auto velocity_state_interface_it = std::find_if(  
           state_interfaces_.begin(   ),   state_interfaces_.end(   ),  
           [](  const hardware_interface::LoanedStateInterface & state_interface ) {
           return state_interface.get_interface_name(   ) == hardware_interface::HW_IF_VELOCITY;
           } );
           if (  velocity_state_interface_it == state_interfaces_.end(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Expected 1 velocity state interface" );
           return controller_interface::CallbackReturn::ERROR;
           }
           if (  velocity_state_interface_it->get_prefix_name(   ) != joint_name_ )
           {
           RCLCPP_ERROR_STREAM(  
           get_node(   )->get_logger(   ),   "Velocity command interface is different than joint name `"
           << velocity_state_interface_it->get_prefix_name(   ) << "` != `"
           << joint_name_ << "`" );
           return controller_interface::CallbackReturn::ERROR;
           }
          
           joint_position_command_interface_ = *position_command_interface_it;
           joint_position_state_interface_ = *position_state_interface_it;
           joint_velocity_state_interface_ = *velocity_state_interface_it;
          
           // Hardware interface adapter
           hw_iface_adapter_.init(  joint_position_command_interface_,   get_node(   ) );
          
           // Command - non RT version
           command_struct_.position_ = joint_position_state_interface_->get(   ).get_value(   );
           command_struct_.max_effort_ = default_max_effort_;
          
           // Result
           pre_alloc_result_ = std::make_shared<control_msgs::action::GripperCommand::Result>(   );
           pre_alloc_result_->position = command_struct_.position_;
           pre_alloc_result_->reached_goal = false;
           pre_alloc_result_->stalled = false;
          
           // Action interface
           action_server_ = rclcpp_action::create_server<control_msgs::action::GripperCommand>(  
           get_node(   ),   "~/gripper_cmd",  
           std::bind(  
           &GripperActionController::goal_callback,   this,   std::placeholders::_1,   std::placeholders::_2 ),  
           std::bind(  &GripperActionController::cancel_callback,   this,   std::placeholders::_1 ),  
           std::bind(  &GripperActionController::accepted_callback,   this,   std::placeholders::_1 ) );
          
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          template <const char * HardwareInterface>
     323  controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_deactivate(  
     324   const rclcpp_lifecycle::State & )
          {
           joint_position_command_interface_ = std::experimental::nullopt;
           joint_position_state_interface_ = std::experimental::nullopt;
           joint_velocity_state_interface_ = std::experimental::nullopt;
           release_interfaces(   );
           return controller_interface::CallbackReturn::SUCCESS;
          }
          
          template <const char * HardwareInterface>
          controller_interface::InterfaceConfiguration
     335  GripperActionController<HardwareInterface>::command_interface_configuration(   ) const
          {
           return {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {joint_name_ + "/" + hardware_interface::HW_IF_POSITION}};
          }
          
          template <const char * HardwareInterface>
          controller_interface::InterfaceConfiguration
     344  GripperActionController<HardwareInterface>::state_interface_configuration(   ) const
          {
           return {
           controller_interface::interface_configuration_type::INDIVIDUAL,  
           {joint_name_ + "/" + hardware_interface::HW_IF_POSITION,  
           joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}};
          }
          
          template <const char * HardwareInterface>
     353  GripperActionController<HardwareInterface>::GripperActionController(   )
          : controller_interface::ControllerInterface(   ),  
           action_monitor_period_(  rclcpp::Duration::from_seconds(  0 ) )
          {
          }
          
          } // namespace gripper_action_controller
          
          #endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_

ros2_controllers/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

          // Copyright 2014,   SRI International
          // Copyright 2013,   PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Sachin Chitta,   Adolfo Rodriguez Tsouroukdissian
          
          #ifndef GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
          #define GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
          
          #include <algorithm>
          #include <cassert>
          #include <memory>
          #include <string>
          #include <vector>
          // TODO(  JafarAbdi ): Remove experimental once the default standard is C++17
          #include "experimental/optional"
          
          #include "control_toolbox/pid.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          
          /**
           * \brief Helper class to simplify integrating the GripperActionController with
           * different hardware interfaces.
           *
           * The GripperActionController outputs position while
           * it is supposed to work with either position or effort commands.
           *
           */
          template <const char * HardwareInterface>
      43  class HardwareInterfaceAdapter
          {
          public:
      46   bool init(  
           std::experimental::optional<
           std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* joint_handle */,  
           std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & /* node */ )
           {
           return false;
           }
          
      54   void starting(  const rclcpp::Time & /* time */ ) {}
      55   void stopping(  const rclcpp::Time & /* time */ ) {}
          
      57   double updateCommand(  
           double /* desired_position */,   double /* desired_velocity */,   double /* error_position */,  
           double /* error_velocity */,   double /* max_allowed_effort */ )
           {
           return 0.0;
           }
          };
          
          /**
           * \brief Adapter for a position-controlled hardware interface. Forwards desired
           * positions as commands.
           */
          template <>
      70  class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
          {
          public:
      73   bool init(  
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           joint_handle,  
           const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */ )
           {
           joint_handle_ = joint_handle;
           return true;
           }
          
      82   void starting(  const rclcpp::Time & /* time */ ) {}
      83   void stopping(  const rclcpp::Time & /* time */ ) {}
          
      85   double updateCommand(  
           double desired_position,   double /* desired_velocity */,   double /* error_position */,  
           double /* error_velocity */,   double max_allowed_effort )
           {
           // Forward desired position to command
           joint_handle_->get(   ).set_value(  desired_position );
           return max_allowed_effort;
           }
          
          private:
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           joint_handle_;
          };
          
          /**
           * \brief Adapter for an effort-controlled hardware interface. Maps position and
           * velocity errors to effort commands through a position PID loop.
           *
           * The following is an example configuration of a controller that uses this
           * adapter. Notice the \p gains entry: \code gripper_controller: type:
           * "gripper_action_controller/GripperActionController" joints: gripper_joint
           * goal_tolerance: 0.01
           * stalled_velocity_threshold: 0.01
           * stall_timeout: 0.2
           * gains:
           * gripper_joint: {p: 200,   d: 1,   i: 5,   i_clamp: 1}
           * \endcode
           */
          template <>
     114  class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
          {
          public:
           template <typename ParameterT>
           auto auto_declare(  
           const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node,   const std::string & name,  
           const ParameterT & default_value )
           {
           if (  !node->has_parameter(  name ) )
           {
           return node->declare_parameter<ParameterT>(  name,   default_value );
           }
           else
           {
           return node->get_parameter(  name ).get_value<ParameterT>(   );
           }
           }
          
           bool init(  
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           joint_handle,  
           const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node )
           {
           joint_handle_ = joint_handle;
           // Init PID gains from ROS parameter server
           const std::string prefix = "gains." + joint_handle_->get(   ).get_name(   );
           const auto k_p = auto_declare<double>(  node,   prefix + ".p",   0.0 );
           const auto k_i = auto_declare<double>(  node,   prefix + ".i",   0.0 );
           const auto k_d = auto_declare<double>(  node,   prefix + ".d",   0.0 );
           const auto i_clamp = auto_declare<double>(  node,   prefix + ".i_clamp",   0.0 );
           // Initialize PID
           pid_ = std::make_shared<control_toolbox::Pid>(  k_p,   k_i,   k_d,   i_clamp,   -i_clamp );
           return true;
           }
          
           void starting(  const rclcpp::Time & /* time */ )
           {
           if (  !joint_handle_ )
           {
           return;
           }
           // Reset PIDs,   zero effort commands
           pid_->reset(   );
           joint_handle_->get(   ).set_value(  0.0 );
           }
          
           void stopping(  const rclcpp::Time & /* time */ ) {}
          
           double updateCommand(  
           double /* desired_position */,   double /* desired_velocity */,   double error_position,  
           double error_velocity,   double max_allowed_effort )
           {
           // Preconditions
           if (  !joint_handle_ )
           {
           return 0.0;
           }
           // Time since the last call to update
           const auto period = std::chrono::steady_clock::now(   ) - last_update_time_;
           // Update PIDs
           double command = pid_->computeCommand(  error_position,   error_velocity,   period.count(   ) );
           command = std::min<double>(  
           fabs(  max_allowed_effort ),   std::max<double>(  -fabs(  max_allowed_effort ),   command ) );
           joint_handle_->get(   ).set_value(  command );
           last_update_time_ = std::chrono::steady_clock::now(   );
           return command;
           }
          
          private:
           using PidPtr = std::shared_ptr<control_toolbox::Pid>;
           PidPtr pid_;
           std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
           joint_handle_;
           std::chrono::steady_clock::time_point last_update_time_;
          };
          
          #endif // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_

ros2_controllers/gripper_controllers/include/gripper_controllers/visibility_control.hpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_
          #define GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__(  (  dllexport ) )
          #define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(  dllexport )
          #define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT
          #else
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT
          #endif
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC
          #define GRIPPER_ACTION_CONTROLLER_LOCAL
          #else
          #define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define GRIPPER_ACTION_CONTROLLER_IMPORT
          #if __GNUC__ >= 4
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC
          #define GRIPPER_ACTION_CONTROLLER_LOCAL
          #endif
          #define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE
          #endif
          
          #endif // GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_

ros2_controllers/gripper_controllers/src/gripper_action_controller.cpp

       1  // Copyright 2014,   SRI International
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /// \author Sachin Chitta
          
          // Project
          #include <gripper_controllers/gripper_action_controller.hpp>
          #include <hardware_interface/types/hardware_interface_type_values.hpp>
          namespace position_controllers
          {
          /**
           * \brief Gripper action controller that sends
           * commands to a \b position interface.
           */
          using GripperActionController =
           gripper_action_controller::GripperActionController<hardware_interface::HW_IF_POSITION>;
          } // namespace position_controllers
          
          namespace effort_controllers
          {
          /**
           * \brief Gripper action controller that sends
           * commands to a \b effort interface.
           */
          using GripperActionController =
           gripper_action_controller::GripperActionController<hardware_interface::HW_IF_EFFORT>;
          } // namespace effort_controllers
          
          #include "pluginlib/class_list_macros.hpp"
          
      42  PLUGINLIB_EXPORT_CLASS(  
           position_controllers::GripperActionController,   controller_interface::ControllerInterface )
          PLUGINLIB_EXPORT_CLASS(  
      45   effort_controllers::GripperActionController,   controller_interface::ControllerInterface )

ros2_controllers/gripper_controllers/test/test_load_gripper_action_controllers.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      23  TEST(  TestLoadGripperActionControllers,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_gripper_action_position_controller",   "position_controllers/GripperActionController" ) );
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_gripper_action_effort_controller",   "effort_controllers/GripperActionController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp

       1  // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #ifndef IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
          #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "controller_interface/controller_interface.hpp"
          #include "imu_sensor_broadcaster/visibility_control.h"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "realtime_tools/realtime_publisher.h"
          #include "semantic_components/imu_sensor.hpp"
          #include "sensor_msgs/msg/imu.hpp"
          
          namespace imu_sensor_broadcaster
          {
      36  class IMUSensorBroadcaster : public controller_interface::ControllerInterface
          {
          public:
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init(   ) override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           IMU_SENSOR_BROADCASTER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
          protected:
           std::string sensor_name_;
           std::string frame_id_;
          
           std::unique_ptr<semantic_components::IMUSensor> imu_sensor_;
          
           using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::Imu>;
           rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr sensor_state_publisher_;
           std::unique_ptr<StatePublisher> realtime_publisher_;
          };
          
          } // namespace imu_sensor_broadcaster
          
          #endif // IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_

ros2_controllers/imu_sensor_broadcaster/include/imu_sensor_broadcaster/visibility_control.h

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Subhas Das,   Denis Stogl
           */
          
          #ifndef IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
          #define IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define IMU_SENSOR_BROADCASTER_EXPORT __attribute__(  (  dllexport ) )
          #define IMU_SENSOR_BROADCASTER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define IMU_SENSOR_BROADCASTER_EXPORT __declspec(  dllexport )
          #define IMU_SENSOR_BROADCASTER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef IMU_SENSOR_BROADCASTER_BUILDING_DLL
          #define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_EXPORT
          #else
          #define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_IMPORT
          #endif
          #define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE IMU_SENSOR_BROADCASTER_PUBLIC
          #define IMU_SENSOR_BROADCASTER_LOCAL
          #else
          #define IMU_SENSOR_BROADCASTER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define IMU_SENSOR_BROADCASTER_IMPORT
          #if __GNUC__ >= 4
          #define IMU_SENSOR_BROADCASTER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define IMU_SENSOR_BROADCASTER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define IMU_SENSOR_BROADCASTER_PUBLIC
          #define IMU_SENSOR_BROADCASTER_LOCAL
          #endif
          #define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE
          #endif
          
          #endif // IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_

ros2_controllers/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

       1  // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
          
          #include <memory>
          #include <string>
          
          namespace imu_sensor_broadcaster
          {
      26  controller_interface::CallbackReturn IMUSensorBroadcaster::on_init(   )
          {
           try
           {
           auto_declare<std::string>(  "sensor_name",   "" );
           auto_declare<std::string>(  "frame_id",   "" );
           }
           catch (  const std::exception & e )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           return CallbackReturn::SUCCESS;
          }
          
      43  controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(  
      44   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           sensor_name_ = get_node(   )->get_parameter(  "sensor_name" ).as_string(   );
           if (  sensor_name_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'sensor_name' parameter has to be specified." );
           return CallbackReturn::ERROR;
           }
          
           frame_id_ = get_node(   )->get_parameter(  "frame_id" ).as_string(   );
           if (  frame_id_.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "'frame_id' parameter has to be provided." );
           return CallbackReturn::ERROR;
           }
          
           imu_sensor_ =
           std::make_unique<semantic_components::IMUSensor>(  semantic_components::IMUSensor(  sensor_name_ ) );
           try
           {
           // register ft sensor data publisher
           sensor_state_publisher_ =
           get_node(   )->create_publisher<sensor_msgs::msg::Imu>(  "~/imu",   rclcpp::SystemDefaultsQoS(   ) );
           realtime_publisher_ = std::make_unique<StatePublisher>(  sensor_state_publisher_ );
           }
           catch (  const std::exception & e )
           {
           fprintf(  
           stderr,   "Exception thrown during publisher creation at configure stage with message : %s \n",  
           e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           realtime_publisher_->lock(   );
           realtime_publisher_->msg_.header.frame_id = frame_id_;
           realtime_publisher_->unlock(   );
          
           RCLCPP_DEBUG(  get_node(   )->get_logger(   ),   "configure successful" );
           return CallbackReturn::SUCCESS;
          }
          
      85  controller_interface::InterfaceConfiguration IMUSensorBroadcaster::command_interface_configuration(   )
           const
          {
           controller_interface::InterfaceConfiguration command_interfaces_config;
           command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
           return command_interfaces_config;
          }
          
      93  controller_interface::InterfaceConfiguration IMUSensorBroadcaster::state_interface_configuration(   )
           const
          {
           controller_interface::InterfaceConfiguration state_interfaces_config;
           state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           state_interfaces_config.names = imu_sensor_->get_state_interface_names(   );
           return state_interfaces_config;
          }
          
     102  controller_interface::CallbackReturn IMUSensorBroadcaster::on_activate(  
     103   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           imu_sensor_->assign_loaned_state_interfaces(  state_interfaces_ );
           return CallbackReturn::SUCCESS;
          }
          
     109  controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate(  
     110   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           imu_sensor_->release_interfaces(   );
           return CallbackReturn::SUCCESS;
          }
          
     116  controller_interface::return_type IMUSensorBroadcaster::update(  
     117   const rclcpp::Time & time,   const rclcpp::Duration & /*period*/ )
          {
           if (  realtime_publisher_ && realtime_publisher_->trylock(   ) )
           {
           realtime_publisher_->msg_.header.stamp = time;
           imu_sensor_->get_values_as_message(  realtime_publisher_->msg_ );
           realtime_publisher_->unlockAndPublish(   );
           }
          
           return controller_interface::return_type::OK;
          }
          
          } // namespace imu_sensor_broadcaster
          
          #include "pluginlib/class_list_macros.hpp"
          
     133  PLUGINLIB_EXPORT_CLASS(  
           imu_sensor_broadcaster::IMUSensorBroadcaster,   controller_interface::ControllerInterface )

ros2_controllers/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp

          // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #include "test_imu_sensor_broadcaster.hpp"
          
          #include <memory>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "sensor_msgs/msg/imu.hpp"
          
          using hardware_interface::LoanedStateInterface;
          
          namespace
          {
      38  constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
          constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
          
          } // namespace
          
          void IMUSensorBroadcasterTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
          void IMUSensorBroadcasterTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
          void IMUSensorBroadcasterTest::SetUp(   )
          {
           // initialize controller
           imu_broadcaster_ = std::make_unique<FriendIMUSensorBroadcaster>(   );
          }
          
          void IMUSensorBroadcasterTest::TearDown(   ) { imu_broadcaster_.reset(  nullptr ); }
          
          void IMUSensorBroadcasterTest::SetUpIMUBroadcaster(   )
          {
           const auto result = imu_broadcaster_->init(  "test_imu_sensor_broadcaster" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedStateInterface> state_ifs;
           state_ifs.emplace_back(  imu_orientation_x_ );
           state_ifs.emplace_back(  imu_orientation_y_ );
           state_ifs.emplace_back(  imu_orientation_z_ );
           state_ifs.emplace_back(  imu_orientation_w_ );
           state_ifs.emplace_back(  imu_angular_velocity_x_ );
           state_ifs.emplace_back(  imu_angular_velocity_y_ );
           state_ifs.emplace_back(  imu_angular_velocity_z_ );
           state_ifs.emplace_back(  imu_linear_acceleration_x_ );
           state_ifs.emplace_back(  imu_linear_acceleration_y_ );
           state_ifs.emplace_back(  imu_linear_acceleration_z_ );
          
           imu_broadcaster_->assign_interfaces(  {},   std::move(  state_ifs ) );
          }
          
          void IMUSensorBroadcasterTest::subscribe_and_get_message(  sensor_msgs::msg::Imu & imu_msg )
          {
           // create a new subscriber
           rclcpp::Node test_subscription_node(  "test_subscription_node" );
           auto subs_callback = [&](  const sensor_msgs::msg::Imu::SharedPtr ) {};
           auto subscription = test_subscription_node.create_subscription<sensor_msgs::msg::Imu>(  
           "/test_imu_sensor_broadcaster/imu",   10,   subs_callback );
          
           // call update to publish the test value
           // since update doesn't guarantee a published message,   republish until received
           int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
           rclcpp::WaitSet wait_set; // block used to wait on message
           wait_set.add_subscription(  subscription );
           while (  max_sub_check_loop_count-- )
           {
           imu_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // check if message has been received
           if (  wait_set.wait(  std::chrono::milliseconds(  2 ) ).kind(   ) == rclcpp::WaitResultKind::Ready )
           {
           break;
           }
           }
           ASSERT_GE(  max_sub_check_loop_count,   0 ) << "Test was unable to publish a message through "
           "controller/broadcaster update loop";
          
           // take message from subscription
           rclcpp::MessageInfo msg_info;
           ASSERT_TRUE(  subscription->take(  imu_msg,   msg_info ) );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_InterfaceNames_NotSet )
          {
           SetUpIMUBroadcaster(   );
          
           // configure failed
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_FrameId_NotSet )
          {
           SetUpIMUBroadcaster(   );
          
           // set the 'interface_names'
           imu_broadcaster_->get_node(   )->set_parameter(  
           {"interface_names.angular_velocity.x",   "imu_sensor/angular_velocity.x"} );
           imu_broadcaster_->get_node(   )->set_parameter(  
           {"interface_names.linear_acceleration.z",   "imu_sensor/linear_acceleration.z"} );
          
           // configure failed,   'frame_id' parameter not set
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   InterfaceNames_FrameId_NotSet )
          {
           SetUpIMUBroadcaster(   );
          
           // set the 'sensor_name'
           imu_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
          
           // configure failed,   'frame_id' parameter not set
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_Configure_Success )
          {
           SetUpIMUBroadcaster(   );
          
           // set the 'sensor_name'
           imu_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
          
           // set the 'frame_id'
           imu_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           // configure passed
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_Activate_Success )
          {
           SetUpIMUBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           imu_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           imu_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           // configure and activate success
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  imu_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_Update_Success )
          {
           SetUpIMUBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           imu_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           imu_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  imu_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  
           imu_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          }
          
          TEST_F(  IMUSensorBroadcasterTest,   SensorName_Publish_Success )
          {
           SetUpIMUBroadcaster(   );
          
           // set the params 'sensor_name' and 'frame_id'
           imu_broadcaster_->get_node(   )->set_parameter(  {"sensor_name",   sensor_name_} );
           imu_broadcaster_->get_node(   )->set_parameter(  {"frame_id",   frame_id_} );
          
           ASSERT_EQ(  imu_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
           ASSERT_EQ(  imu_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           sensor_msgs::msg::Imu imu_msg;
           subscribe_and_get_message(  imu_msg );
          
           ASSERT_EQ(  imu_msg.header.frame_id,   frame_id_ );
           ASSERT_EQ(  imu_msg.orientation.x,   sensor_values_[0] );
           ASSERT_EQ(  imu_msg.orientation.y,   sensor_values_[1] );
           ASSERT_EQ(  imu_msg.orientation.z,   sensor_values_[2] );
           ASSERT_EQ(  imu_msg.orientation.w,   sensor_values_[3] );
           ASSERT_EQ(  imu_msg.angular_velocity.x,   sensor_values_[4] );
           ASSERT_EQ(  imu_msg.angular_velocity.y,   sensor_values_[5] );
           ASSERT_EQ(  imu_msg.angular_velocity.z,   sensor_values_[6] );
           ASSERT_EQ(  imu_msg.linear_acceleration.x,   sensor_values_[7] );
           ASSERT_EQ(  imu_msg.linear_acceleration.y,   sensor_values_[8] );
           ASSERT_EQ(  imu_msg.linear_acceleration.z,   sensor_values_[9] );
          }

ros2_controllers/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp

          // Copyright 2021 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Authors: Subhas Das,   Denis Stogl,   Victor Lopez
           */
          
          #ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_
          #define TEST_IMU_SENSOR_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          
          #include "gmock/gmock.h"
          
          #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
          
          // subclassing and friending so we can access member variables
      30  class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster::IMUSensorBroadcaster
          {
      32   FRIEND_TEST(  IMUSensorBroadcasterTest,   SensorNameParameterNotSet );
      33   FRIEND_TEST(  IMUSensorBroadcasterTest,   InterfaceNamesParameterNotSet );
      34   FRIEND_TEST(  IMUSensorBroadcasterTest,   FrameIdParameterNotSet );
      35   FRIEND_TEST(  IMUSensorBroadcasterTest,   SensorNameParameterIsEmpty );
      36   FRIEND_TEST(  IMUSensorBroadcasterTest,   InterfaceNameParameterIsEmpty );
          
      38   FRIEND_TEST(  IMUSensorBroadcasterTest,   ActivateSuccess );
      39   FRIEND_TEST(  IMUSensorBroadcasterTest,   UpdateTest );
      40   FRIEND_TEST(  IMUSensorBroadcasterTest,   SensorStatePublishTest );
          };
          
      43  class IMUSensorBroadcasterTest : public ::testing::Test
          {
          public:
      46   static void SetUpTestCase(   );
      47   static void TearDownTestCase(   );
          
      49   void SetUp(   );
      50   void TearDown(   );
          
      52   void SetUpIMUBroadcaster(   );
          
          protected:
      55   const std::string sensor_name_ = "imu_sensor";
      56   const std::string frame_id_ = "imu_sensor_frame";
      57   std::array<double,   10> sensor_values_ = {1.1,   2.2,   3.3,   4.4,   5.5,   6.6,   7.7,   8.8,   9.9,   10.10};
           hardware_interface::StateInterface imu_orientation_x_{
           sensor_name_,   "orientation.x",   &sensor_values_[0]};
           hardware_interface::StateInterface imu_orientation_y_{
      61   sensor_name_,   "orientation.y",   &sensor_values_[1]};
           hardware_interface::StateInterface imu_orientation_z_{
           sensor_name_,   "orientation.z",   &sensor_values_[2]};
           hardware_interface::StateInterface imu_orientation_w_{
           sensor_name_,   "orientation.w",   &sensor_values_[3]};
           hardware_interface::StateInterface imu_angular_velocity_x_{
           sensor_name_,   "angular_velocity.x",   &sensor_values_[4]};
           hardware_interface::StateInterface imu_angular_velocity_y_{
           sensor_name_,   "angular_velocity.y",   &sensor_values_[5]};
           hardware_interface::StateInterface imu_angular_velocity_z_{
           sensor_name_,   "angular_velocity.z",   &sensor_values_[6]};
           hardware_interface::StateInterface imu_linear_acceleration_x_{
           sensor_name_,   "linear_acceleration.x",   &sensor_values_[7]};
           hardware_interface::StateInterface imu_linear_acceleration_y_{
           sensor_name_,   "linear_acceleration.y",   &sensor_values_[8]};
           hardware_interface::StateInterface imu_linear_acceleration_z_{
           sensor_name_,   "linear_acceleration.z",   &sensor_values_[9]};
          
           std::unique_ptr<FriendIMUSensorBroadcaster> imu_broadcaster_;
          
           void subscribe_and_get_message(  sensor_msgs::msg::Imu & imu_msg );
          };
          
          #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_

ros2_controllers/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp

       1  // Copyright (  c ) 2021,   Stogl Robotics Consulting UG (  haftungsbeschränkt )
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /*
           * Author: Subhas Das,   Denis Stogl
           */
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      29  TEST(  TestLoadIMUSensorBroadcaster,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_imu_sensor_broadcaster",   "imu_sensor_broadcaster/IMUSensorBroadcaster" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
          #define JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "control_msgs/msg/dynamic_joint_state.hpp"
          #include "controller_interface/controller_interface.hpp"
          #include "joint_state_broadcaster/visibility_control.h"
          #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "realtime_tools/realtime_publisher.h"
          #include "sensor_msgs/msg/joint_state.hpp"
          
          namespace joint_state_broadcaster
          {
          /**
           * \brief Joint State Broadcaster for all or some state in a ros2_control system.
           *
           * JointStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
           * There is a possibility to publish all available states (  typical use ),   or only specific ones.
           * The latter is,   for example,   used when hardware provides multiple measurement sources for some
           * of its states,   e.g.,   position.
           * It is possible to define a mapping of measurements
           * from different sources stored in custom interfaces to standard dynamic names in JointState
           * message.
           * If "joints" or "interfaces" parameter is empty,   all available states are published.
           *
           * \param use_local_topics Flag to publish topics in local namespace.
           * \param joints Names of the joints to publish.
           * \param interfaces Names of interfaces to publish.
           * \param map_interface_to_joint_state.{HW_IF_POSITION|HW_IF_VELOCITY|HW_IF_EFFORT} mapping
           * between custom interface names and standard names in sensor_msgs::msg::JointState message.
           *
           * Publishes to:
           * - \b joint_states (  sensor_msgs::msg::JointState ): Joint states related to movement
           * (  position,   velocity,   effort ).
           * - \b dynamic_joint_states (  control_msgs::msg::DynamicJointState ): Joint states regardless of
           * its interface type.
           */
      57  class JointStateBroadcaster : public controller_interface::ControllerInterface
          {
          public:
           JOINT_STATE_BROADCASTER_PUBLIC
      61   JointStateBroadcaster(   );
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_STATE_BROADCASTER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
          protected:
           bool init_joint_data(   );
           void init_joint_state_msg(   );
           void init_dynamic_joint_state_msg(   );
           bool use_all_available_interfaces(   ) const;
          
          protected:
           // Optional parameters
           bool use_local_topics_;
           std::vector<std::string> joints_;
           std::vector<std::string> interfaces_;
           std::unordered_map<std::string,   std::string> map_interface_to_joint_state_;
          
           // For the JointState message,  
           // we store the name of joints with compatible interfaces
           std::vector<std::string> joint_names_;
           std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> joint_state_publisher_;
           std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>
           realtime_joint_state_publisher_;
          
           // For the DynamicJointState format,   we use a map to buffer values in for easier lookup
           // This allows to preserve whatever order or names/interfaces were initialized.
           std::unordered_map<std::string,   std::unordered_map<std::string,   double>> name_if_value_mapping_;
           std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
           dynamic_joint_state_publisher_;
           std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
           realtime_dynamic_joint_state_publisher_;
          };
          
          } // namespace joint_state_broadcaster
          
          #endif // JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_

ros2_controllers/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_
          #define JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define JOINT_STATE_BROADCASTER_EXPORT __attribute__(  (  dllexport ) )
          #define JOINT_STATE_BROADCASTER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define JOINT_STATE_BROADCASTER_EXPORT __declspec(  dllexport )
          #define JOINT_STATE_BROADCASTER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef JOINT_STATE_BROADCASTER_BUILDING_DLL
          #define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_EXPORT
          #else
          #define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_IMPORT
          #endif
          #define JOINT_STATE_BROADCASTER_PUBLIC_TYPE JOINT_STATE_BROADCASTER_PUBLIC
          #define JOINT_STATE_BROADCASTER_LOCAL
          #else
          #define JOINT_STATE_BROADCASTER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define JOINT_STATE_BROADCASTER_IMPORT
          #if __GNUC__ >= 4
          #define JOINT_STATE_BROADCASTER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define JOINT_STATE_BROADCASTER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define JOINT_STATE_BROADCASTER_PUBLIC
          #define JOINT_STATE_BROADCASTER_LOCAL
          #endif
          #define JOINT_STATE_BROADCASTER_PUBLIC_TYPE
          #endif
          
          #endif // JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_

ros2_controllers/joint_state_broadcaster/src/joint_state_broadcaster.cpp

          // Copyright 2021 ros2_control development team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "joint_state_broadcaster/joint_state_broadcaster.hpp"
          
          #include <stddef.h>
          #include <limits>
          #include <memory>
          #include <string>
          #include <unordered_map>
          #include <vector>
          
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "rclcpp/clock.hpp"
          #include "rclcpp/qos.hpp"
          #include "rclcpp/qos_event.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "rcpputils/split.hpp"
          #include "rcutils/logging_macros.h"
          #include "std_msgs/msg/header.hpp"
          
          namespace rclcpp_lifecycle
          {
      37  class State;
          } // namespace rclcpp_lifecycle
          
          namespace joint_state_broadcaster
          {
      42  const auto kUninitializedValue = std::numeric_limits<double>::quiet_NaN(   );
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          
      47  JointStateBroadcaster::JointStateBroadcaster(   ) {}
          
      49  controller_interface::CallbackReturn JointStateBroadcaster::on_init(   )
          {
           try
           {
           auto_declare<bool>(  "use_local_topics",   false );
           auto_declare<std::vector<std::string>>(  "joints",   std::vector<std::string>(  {} ) );
           auto_declare<std::vector<std::string>>(  "interfaces",   std::vector<std::string>(  {} ) );
           auto_declare<std::string>(  
           std::string(  "map_interface_to_joint_state." ) + HW_IF_POSITION,   HW_IF_POSITION );
           auto_declare<std::string>(  
           std::string(  "map_interface_to_joint_state." ) + HW_IF_VELOCITY,   HW_IF_VELOCITY );
           auto_declare<std::string>(  
           std::string(  "map_interface_to_joint_state." ) + HW_IF_EFFORT,   HW_IF_EFFORT );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           return CallbackReturn::SUCCESS;
          }
          
          controller_interface::InterfaceConfiguration
      73  JointStateBroadcaster::command_interface_configuration(   ) const
          {
           return controller_interface::InterfaceConfiguration{
           controller_interface::interface_configuration_type::NONE};
          }
          
      79  controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interface_configuration(   )
           const
          {
           controller_interface::InterfaceConfiguration state_interfaces_config;
          
           if (  use_all_available_interfaces(   ) )
           {
           state_interfaces_config.type = controller_interface::interface_configuration_type::ALL;
           }
           else
           {
           state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           for (  const auto & joint : joints_ )
           {
           for (  const auto & interface : interfaces_ )
           {
           state_interfaces_config.names.push_back(  joint + "/" + interface );
           }
           }
           }
          
           return state_interfaces_config;
          }
          
     103  controller_interface::CallbackReturn JointStateBroadcaster::on_configure(  
     104   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           use_local_topics_ = get_node(   )->get_parameter(  "use_local_topics" ).as_bool(   );
           joints_ = get_node(   )->get_parameter(  "joints" ).as_string_array(   );
           interfaces_ = get_node(   )->get_parameter(  "interfaces" ).as_string_array(   );
          
           if (  use_all_available_interfaces(   ) )
           {
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),  
           "'joints' or 'interfaces' parameter is empty. "
           "All available state interfaces will be published" );
           joints_.clear(   );
           interfaces_.clear(   );
           }
           else
           {
           RCLCPP_INFO(  
           get_node(   )->get_logger(   ),  
           "Publishing state interfaces defined in 'joints' and 'interfaces' parameters." );
           }
          
           auto get_map_interface_parameter = [&](  const std::string & interface ) {
           std::string interface_to_map =
           get_node(   )
           ->get_parameter(  std::string(  "map_interface_to_joint_state." ) + interface )
           .as_string(   );
          
           if (  std::find(  interfaces_.begin(   ),   interfaces_.end(   ),   interface ) != interfaces_.end(   ) )
           {
           map_interface_to_joint_state_[interface] = interface;
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),  
           "Mapping from '%s' to interface '%s' will not be done,   because '%s' is defined "
           "in 'interface' parameter.",  
           interface_to_map.c_str(   ),   interface.c_str(   ),   interface.c_str(   ) );
           }
           else
           {
           map_interface_to_joint_state_[interface_to_map] = interface;
           }
           };
          
           map_interface_to_joint_state_ = {};
           get_map_interface_parameter(  HW_IF_POSITION );
           get_map_interface_parameter(  HW_IF_VELOCITY );
           get_map_interface_parameter(  HW_IF_EFFORT );
          
           try
           {
           const std::string topic_name_prefix = use_local_topics_ ? "~/" : "";
          
           joint_state_publisher_ = get_node(   )->create_publisher<sensor_msgs::msg::JointState>(  
           topic_name_prefix + "joint_states",   rclcpp::SystemDefaultsQoS(   ) );
          
           realtime_joint_state_publisher_ =
           std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(  
           joint_state_publisher_ );
          
           dynamic_joint_state_publisher_ =
           get_node(   )->create_publisher<control_msgs::msg::DynamicJointState>(  
           topic_name_prefix + "dynamic_joint_states",   rclcpp::SystemDefaultsQoS(   ) );
          
           realtime_dynamic_joint_state_publisher_ =
           std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(  
           dynamic_joint_state_publisher_ );
           }
           catch (  const std::exception & e )
           {
           // get_node(   ) may throw,   logging raw here
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::SUCCESS;
          }
          
     180  controller_interface::CallbackReturn JointStateBroadcaster::on_activate(  
     181   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           if (  !init_joint_data(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "None of requested interfaces exist. Controller will not run." );
           return CallbackReturn::ERROR;
           }
          
           init_joint_state_msg(   );
           init_dynamic_joint_state_msg(   );
          
           if (  
           !use_all_available_interfaces(   ) &&
           state_interfaces_.size(   ) != (  joints_.size(   ) * interfaces_.size(   ) ) )
           {
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),  
           "Not all requested interfaces exists. "
           "Check ControllerManager output for more detailed information." );
           }
          
           return CallbackReturn::SUCCESS;
          }
          
     206  controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate(  
     207   const rclcpp_lifecycle::State & /*previous_state*/ )
          {
           return CallbackReturn::SUCCESS;
          }
          
          template <typename T>
     213  bool has_any_key(  
           const std::unordered_map<std::string,   T> & map,   const std::vector<std::string> & keys )
          {
           bool found_key = false;
           for (  const auto & key_item : map )
           {
           const auto & key = key_item.first;
           if (  std::find(  keys.cbegin(   ),   keys.cend(   ),   key ) != keys.cend(   ) )
           {
           found_key = true;
           break;
           }
           }
           return found_key;
          }
          
     229  bool JointStateBroadcaster::init_joint_data(   )
          {
           if (  state_interfaces_.empty(   ) )
           {
           return false;
           }
          
           // loop in reverse order,   this maintains the order of values at retrieval time
           for (  auto si = state_interfaces_.crbegin(   ); si != state_interfaces_.crend(   ); si++ )
           {
           // initialize map if name is new
           if (  name_if_value_mapping_.count(  si->get_prefix_name(   ) ) == 0 )
           {
           name_if_value_mapping_[si->get_prefix_name(   )] = {};
           }
           // add interface name
           std::string interface_name = si->get_interface_name(   );
           if (  map_interface_to_joint_state_.count(  interface_name ) > 0 )
           {
           interface_name = map_interface_to_joint_state_[interface_name];
           }
           name_if_value_mapping_[si->get_prefix_name(   )][interface_name] = kUninitializedValue;
           }
          
           // filter state interfaces that have at least one of the joint_states fields,  
           // the rest will be ignored for this message
           for (  const auto & name_ifv : name_if_value_mapping_ )
           {
           const auto & interfaces_and_values = name_ifv.second;
           if (  has_any_key(  interfaces_and_values,   {HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_EFFORT} ) )
           {
           joint_names_.push_back(  name_ifv.first );
           }
           }
          
           // Add extra joints from parameters,   each joint will be added to joint_names_ and
           // name_if_value_mapping_ if it is not already there
           rclcpp::Parameter extra_joints;
           if (  get_node(   )->get_parameter(  "extra_joints",   extra_joints ) )
           {
           const std::vector<std::string> & extra_joints_names = extra_joints.as_string_array(   );
           for (  const auto & extra_joint_name : extra_joints_names )
           {
           if (  name_if_value_mapping_.count(  extra_joint_name ) == 0 )
           {
           name_if_value_mapping_[extra_joint_name] = {
           {HW_IF_POSITION,   0.0},   {HW_IF_VELOCITY,   0.0},   {HW_IF_EFFORT,   0.0}};
           joint_names_.push_back(  extra_joint_name );
           }
           }
           }
          
           return true;
          }
          
     284  void JointStateBroadcaster::init_joint_state_msg(   )
          {
           const size_t num_joints = joint_names_.size(   );
          
           /// @note joint_state_msg publishes position,   velocity and effort for all joints,  
           /// with at least one of these interfaces,   the rest are omitted from this message
          
           // default initialization for joint state message
           auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
           joint_state_msg.name = joint_names_;
           joint_state_msg.position.resize(  num_joints,   kUninitializedValue );
           joint_state_msg.velocity.resize(  num_joints,   kUninitializedValue );
           joint_state_msg.effort.resize(  num_joints,   kUninitializedValue );
          }
          
     299  void JointStateBroadcaster::init_dynamic_joint_state_msg(   )
          {
           auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
           for (  const auto & name_ifv : name_if_value_mapping_ )
           {
           const auto & name = name_ifv.first;
           const auto & interfaces_and_values = name_ifv.second;
           dynamic_joint_state_msg.joint_names.push_back(  name );
           control_msgs::msg::InterfaceValue if_value;
           for (  const auto & interface_and_value : interfaces_and_values )
           {
           if_value.interface_names.emplace_back(  interface_and_value.first );
           if_value.values.emplace_back(  kUninitializedValue );
           }
           dynamic_joint_state_msg.interface_values.emplace_back(  if_value );
           }
          }
          
     317  bool JointStateBroadcaster::use_all_available_interfaces(   ) const
          {
           return joints_.empty(   ) || interfaces_.empty(   );
          }
          
     322  double get_value(  
           const std::unordered_map<std::string,   std::unordered_map<std::string,   double>> & map,  
           const std::string & name,   const std::string & interface_name )
          {
           const auto & interfaces_and_values = map.at(  name );
           const auto interface_and_value = interfaces_and_values.find(  interface_name );
           if (  interface_and_value != interfaces_and_values.cend(   ) )
           {
           return interface_and_value->second;
           }
           else
           {
           return kUninitializedValue;
           }
          }
          
     338  controller_interface::return_type JointStateBroadcaster::update(  
     339   const rclcpp::Time & time,   const rclcpp::Duration & /*period*/ )
          {
           for (  const auto & state_interface : state_interfaces_ )
           {
           std::string interface_name = state_interface.get_interface_name(   );
           if (  map_interface_to_joint_state_.count(  interface_name ) > 0 )
           {
           interface_name = map_interface_to_joint_state_[interface_name];
           }
           name_if_value_mapping_[state_interface.get_prefix_name(   )][interface_name] =
           state_interface.get_value(   );
           RCLCPP_DEBUG(  
           get_node(   )->get_logger(   ),   "%s: %f\n",   state_interface.get_name(   ).c_str(   ),  
           state_interface.get_value(   ) );
           }
          
           if (  realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock(   ) )
           {
           auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
          
           joint_state_msg.header.stamp = time;
          
           // update joint state message and dynamic joint state message
           for (  size_t i = 0; i < joint_names_.size(   ); ++i )
           {
           joint_state_msg.position[i] =
           get_value(  name_if_value_mapping_,   joint_names_[i],   HW_IF_POSITION );
           joint_state_msg.velocity[i] =
           get_value(  name_if_value_mapping_,   joint_names_[i],   HW_IF_VELOCITY );
           joint_state_msg.effort[i] = get_value(  name_if_value_mapping_,   joint_names_[i],   HW_IF_EFFORT );
           }
           realtime_joint_state_publisher_->unlockAndPublish(   );
           }
          
           if (  realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock(   ) )
           {
           auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
           dynamic_joint_state_msg.header.stamp = time;
           for (  size_t joint_index = 0; joint_index < dynamic_joint_state_msg.joint_names.size(   );
           ++joint_index )
           {
           const auto & name = dynamic_joint_state_msg.joint_names[joint_index];
           for (  size_t interface_index = 0;
           interface_index <
           dynamic_joint_state_msg.interface_values[joint_index].interface_names.size(   );
           ++interface_index )
           {
           const auto & interface_name =
           dynamic_joint_state_msg.interface_values[joint_index].interface_names[interface_index];
           dynamic_joint_state_msg.interface_values[joint_index].values[interface_index] =
           name_if_value_mapping_[name][interface_name];
           }
           }
           realtime_dynamic_joint_state_publisher_->unlockAndPublish(   );
           }
          
           return controller_interface::return_type::OK;
          }
          
          } // namespace joint_state_broadcaster
          
          #include "pluginlib/class_list_macros.hpp"
          
          PLUGINLIB_EXPORT_CLASS(  
           joint_state_broadcaster::JointStateBroadcaster,   controller_interface::ControllerInterface )

ros2_controllers/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stddef.h>
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "hardware_interface/loaned_state_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "joint_state_broadcaster/joint_state_broadcaster.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "test_joint_state_broadcaster.hpp"
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          using hardware_interface::LoanedStateInterface;
          using std::placeholders::_1;
          using testing::Each;
          using testing::ElementsAreArray;
          using testing::IsEmpty;
          using testing::SizeIs;
          
          namespace
          {
      46  constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
          constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
          } // namespace
          
          void JointStateBroadcasterTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
          void JointStateBroadcasterTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
          void JointStateBroadcasterTest::SetUp(   )
          {
           // initialize broadcaster
           state_broadcaster_ = std::make_unique<FriendJointStateBroadcaster>(   );
          }
          
          void JointStateBroadcasterTest::TearDown(   ) { state_broadcaster_.reset(  nullptr ); }
          
          void JointStateBroadcasterTest::SetUpStateBroadcaster(  
           const std::vector<std::string> & joint_names,   const std::vector<std::string> & interfaces )
          {
           init_broadcaster_and_set_parameters(  joint_names,   interfaces );
           assign_state_interfaces(  joint_names,   interfaces );
          }
          
          void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(  
           const std::vector<std::string> & joint_names,   const std::vector<std::string> & interfaces )
          {
           const auto result = state_broadcaster_->init(  "joint_state_broadcaster" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           state_broadcaster_->get_node(   )->set_parameter(  {"joints",   joint_names} );
           state_broadcaster_->get_node(   )->set_parameter(  {"interfaces",   interfaces} );
          }
          
          void JointStateBroadcasterTest::assign_state_interfaces(  
           const std::vector<std::string> & joint_names,   const std::vector<std::string> & interfaces )
          {
           std::vector<LoanedStateInterface> state_ifs;
          
           if (  joint_names.empty(   ) || interfaces.empty(   ) )
           {
           state_ifs.emplace_back(  joint_1_pos_state_ );
           state_ifs.emplace_back(  joint_2_pos_state_ );
           state_ifs.emplace_back(  joint_3_pos_state_ );
           state_ifs.emplace_back(  joint_1_vel_state_ );
           state_ifs.emplace_back(  joint_2_vel_state_ );
           state_ifs.emplace_back(  joint_3_vel_state_ );
           state_ifs.emplace_back(  joint_1_eff_state_ );
           state_ifs.emplace_back(  joint_2_eff_state_ );
           state_ifs.emplace_back(  joint_3_eff_state_ );
           }
           else
           {
           for (  const auto & joint : joint_names )
           {
           for (  const auto & interface : interfaces )
           {
           if (  joint == joint_names_[0] && interface == interface_names_[0] )
           {
           state_ifs.emplace_back(  joint_1_pos_state_ );
           }
           if (  joint == joint_names_[1] && interface == interface_names_[0] )
           {
           state_ifs.emplace_back(  joint_2_pos_state_ );
           }
           if (  joint == joint_names_[2] && interface == interface_names_[0] )
           {
           state_ifs.emplace_back(  joint_3_pos_state_ );
           }
           if (  joint == joint_names_[0] && interface == interface_names_[1] )
           {
           state_ifs.emplace_back(  joint_1_vel_state_ );
           }
           if (  joint == joint_names_[1] && interface == interface_names_[1] )
           {
           state_ifs.emplace_back(  joint_2_vel_state_ );
           }
           if (  joint == joint_names_[2] && interface == interface_names_[1] )
           {
           state_ifs.emplace_back(  joint_3_vel_state_ );
           }
           if (  joint == joint_names_[0] && interface == interface_names_[2] )
           {
           state_ifs.emplace_back(  joint_1_eff_state_ );
           }
           if (  joint == joint_names_[1] && interface == interface_names_[2] )
           {
           state_ifs.emplace_back(  joint_2_eff_state_ );
           }
           if (  joint == joint_names_[2] && interface == interface_names_[2] )
           {
           state_ifs.emplace_back(  joint_3_eff_state_ );
           }
           if (  interface == custom_interface_name_ )
           {
           state_ifs.emplace_back(  joint_X_custom_state );
           }
           }
           }
           }
          
           state_broadcaster_->assign_interfaces(  {},   std::move(  state_ifs ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ConfigureErrorTest )
          {
           // publishers not initialized yet
           ASSERT_FALSE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_FALSE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // configure failed
           ASSERT_THROW(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   std::exception );
          
           SetUpStateBroadcaster(   );
           // check state remains unchanged
          
           // publishers still not initialized
           ASSERT_FALSE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_FALSE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTest )
          {
           // publishers not initialized yet
           ASSERT_FALSE(  state_broadcaster_->joint_state_publisher_ );
           ASSERT_FALSE(  state_broadcaster_->dynamic_joint_state_publisher_ );
          
           SetUpStateBroadcaster(   );
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = joint_names_.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[1].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[2].interface_names,  
           ElementsAreArray(  interface_names_ ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestWithoutJointsParameter )
          {
           const std::vector<std::string> JOINT_NAMES = {};
           const std::vector<std::string> IF_NAMES = {interface_names_[0]};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = joint_names_.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[1].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[2].interface_names,  
           ElementsAreArray(  interface_names_ ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestWithoutInterfacesParameter )
          {
           const std::vector<std::string> JOINT_NAMES = {"joint1"};
           const std::vector<std::string> IF_NAMES = {};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = joint_names_.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  joint_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[1].interface_names,  
           ElementsAreArray(  interface_names_ ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[2].interface_names,  
           ElementsAreArray(  interface_names_ ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestTwoJointsOneInterface )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0],   joint_names_[1]};
           const std::vector<std::string> IF_NAMES = {interface_names_[0]};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.velocity[i] ) );
           }
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.effort[i] ) );
           }
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,   ElementsAreArray(  IF_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[1].interface_names,   ElementsAreArray(  IF_NAMES ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestOneJointTwoInterfaces )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
           const std::vector<std::string> IF_NAMES = {interface_names_[0],   interface_names_[1]};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.effort[i] ) );
           }
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,   ElementsAreArray(  IF_NAMES ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestTwoJointTwoInterfacesAllMissing )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0],   joint_names_[1]};
           const std::vector<std::string> IF_NAMES = {interface_names_[0],   interface_names_[1]};
          
           init_broadcaster_and_set_parameters(  JOINT_NAMES,   {interface_names_[0],   interface_names_[1]} );
          
           // assign state with interfaces which are not set in parameters --> We should actually not assign
           // anything because CM will also not do that
           // assign_state_interfaces(  JOINT_NAMES,   {interface_names_[2]} );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           // is none of requested interfaces do not exist,   the controller will not be activated
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_ERROR );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ActivateTestTwoJointTwoInterfacesOneMissing )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0],   joint_names_[1]};
           const std::vector<std::string> IF_NAMES = {interface_names_[0],   interface_names_[1]};
          
           init_broadcaster_and_set_parameters(  JOINT_NAMES,   {interface_names_[0],   interface_names_[1]} );
          
           // Manually assign existing interfaces --> one we need is missing
           std::vector<LoanedStateInterface> state_ifs;
          
           state_ifs.emplace_back(  joint_1_pos_state_ );
           // Missing Joint 1 vel state interface
           state_ifs.emplace_back(  joint_2_pos_state_ );
           state_ifs.emplace_back(  joint_2_vel_state_ );
          
           state_broadcaster_->assign_interfaces(  {},   std::move(  state_ifs ) );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           // here a warning output is expected!
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
           // velocity for joint 1 should be nan because state interface does not exit
           ASSERT_TRUE(  std::isnan(  joint_state_msg.velocity[0] ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.effort[i] ) );
           }
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  {IF_NAMES[0]} ) ); // joint 1 has only pos interface
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[1].interface_names,   ElementsAreArray(  IF_NAMES ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   TestCustomInterfaceWithoutMapping )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
           const std::vector<std::string> IF_NAMES = {custom_interface_name_};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   SizeIs(  0 ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  0 ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  0 ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  0 ) );
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,   ElementsAreArray(  IF_NAMES ) );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->dynamic_joint_state_publisher_ );
          }
          
          TEST_F(  JointStateBroadcasterTest,   TestCustomInterfaceMapping )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
           const std::vector<std::string> IF_NAMES = {custom_interface_name_};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           state_broadcaster_->get_node(   )->set_parameter(  
           {std::string(  "map_interface_to_joint_state." ) + HW_IF_POSITION,   custom_interface_name_} );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.velocity[i] ) );
           }
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.effort[i] ) );
           }
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  {HW_IF_POSITION} ) ); // mapping to this value
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->dynamic_joint_state_publisher_ );
          }
          
          TEST_F(  JointStateBroadcasterTest,   TestCustomInterfaceMappingUpdate )
          {
           const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
           const std::vector<std::string> IF_NAMES = {custom_interface_name_};
           SetUpStateBroadcaster(  JOINT_NAMES,   IF_NAMES );
          
           state_broadcaster_->get_node(   )->set_parameter(  
           {std::string(  "map_interface_to_joint_state." ) + HW_IF_POSITION,   custom_interface_name_} );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  
           state_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           const size_t NUM_JOINTS = JOINT_NAMES.size(   );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_EQ(  joint_state_msg.position[0],   custom_joint_value_ );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.velocity[i] ) );
           }
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
           for (  auto i = 0ul; i < NUM_JOINTS; ++i )
           {
           ASSERT_TRUE(  std::isnan(  joint_state_msg.effort[i] ) );
           }
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.interface_values,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   ElementsAreArray(  JOINT_NAMES ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[0].interface_names,  
           ElementsAreArray(  {HW_IF_POSITION} ) );
          
           // publishers initialized
           ASSERT_TRUE(  state_broadcaster_->joint_state_publisher_ );
           ASSERT_TRUE(  state_broadcaster_->dynamic_joint_state_publisher_ );
          }
          
          TEST_F(  JointStateBroadcasterTest,   UpdateTest )
          {
           SetUpStateBroadcaster(   );
          
           auto node_state = state_broadcaster_->get_node(   )->configure(   );
           node_state = state_broadcaster_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
           ASSERT_EQ(  
           state_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          }
          
          void JointStateBroadcasterTest::test_published_joint_state_message(  const std::string & topic )
          {
           auto node_state = state_broadcaster_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = state_broadcaster_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           rclcpp::Node test_node(  "test_node" );
           auto subs_callback = [&](  const sensor_msgs::msg::JointState::SharedPtr ) {};
           auto subscription =
           test_node.create_subscription<sensor_msgs::msg::JointState>(  topic,   10,   subs_callback );
          
           // call update to publish the test value
           // since update doesn't guarantee a published message,   republish until received
           int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
           rclcpp::WaitSet wait_set; // block used to wait on message
           wait_set.add_subscription(  subscription );
           while (  max_sub_check_loop_count-- )
           {
           state_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // check if message has been received
           if (  wait_set.wait(  std::chrono::milliseconds(  2 ) ).kind(   ) == rclcpp::WaitResultKind::Ready )
           {
           break;
           }
           }
           ASSERT_GE(  max_sub_check_loop_count,   0 ) << "Test was unable to publish a message through "
           "controller/broadcaster update loop";
          
           // take message from subscription
           sensor_msgs::msg::JointState joint_state_msg;
           rclcpp::MessageInfo msg_info;
           ASSERT_TRUE(  subscription->take(  joint_state_msg,   msg_info ) );
          
           const size_t NUM_JOINTS = joint_names_.size(   );
           ASSERT_THAT(  joint_state_msg.name,   SizeIs(  NUM_JOINTS ) );
           // the order in the message may be different
           // we only check that all values in this test are present in the message
           // and that it is the same across the interfaces
           // for test purposes they are mapped to the same doubles
           ASSERT_THAT(  joint_state_msg.position,   ElementsAreArray(  joint_values_ ) );
           ASSERT_THAT(  joint_state_msg.velocity,   ElementsAreArray(  joint_state_msg.position ) );
           ASSERT_THAT(  joint_state_msg.effort,   ElementsAreArray(  joint_state_msg.position ) );
          }
          
          TEST_F(  JointStateBroadcasterTest,   JointStatePublishTest )
          {
           SetUpStateBroadcaster(   );
          
           test_published_joint_state_message(  "joint_states" );
          }
          
          TEST_F(  JointStateBroadcasterTest,   JointStatePublishTestLocalTopic )
          {
           SetUpStateBroadcaster(   );
           state_broadcaster_->get_node(   )->set_parameter(  {"use_local_topics",   true} );
          
           test_published_joint_state_message(  "joint_state_broadcaster/joint_states" );
          }
          
          void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(  
           const std::string & topic )
          {
           auto node_state = state_broadcaster_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = state_broadcaster_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           rclcpp::Node test_node(  "test_node" );
           auto subs_callback = [&](  const control_msgs::msg::DynamicJointState::SharedPtr ) {};
           auto subscription =
           test_node.create_subscription<control_msgs::msg::DynamicJointState>(  topic,   10,   subs_callback );
          
           // call update to publish the test value
           // since update doesn't guarantee a published message,   republish until received
           int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
           rclcpp::WaitSet wait_set; // block used to wait on message
           wait_set.add_subscription(  subscription );
           while (  max_sub_check_loop_count-- )
           {
           state_broadcaster_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // check if message has been received
           if (  wait_set.wait(  std::chrono::milliseconds(  2 ) ).kind(   ) == rclcpp::WaitResultKind::Ready )
           {
           break;
           }
           }
           ASSERT_GE(  max_sub_check_loop_count,   0 ) << "Test was unable to publish a message through "
           "controller/broadcaster update loop";
          
           // take message from subscription
           control_msgs::msg::DynamicJointState dynamic_joint_state_msg;
           rclcpp::MessageInfo msg_info;
           ASSERT_TRUE(  subscription->take(  dynamic_joint_state_msg,   msg_info ) );
          
           const size_t NUM_JOINTS = 3;
           const auto INTERFACE_NAMES = {HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_EFFORT};
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
           // the order in the message may be different
           // we only check that all values in this test are present in the message
           // and that it is the same across the interfaces
           // for test purposes they are mapped to the same doubles
           for (  size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(   ); ++i )
           {
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[i].interface_names,  
           ElementsAreArray(  INTERFACE_NAMES ) );
           const auto index_in_original_order = std::distance(  
           joint_names_.cbegin(   ),  
           std::find(  
           joint_names_.cbegin(   ),   joint_names_.cend(   ),   dynamic_joint_state_msg.joint_names[i] ) );
           ASSERT_THAT(  
           dynamic_joint_state_msg.interface_values[i].values,  
           Each(  joint_values_[index_in_original_order] ) );
           }
          }
          
          TEST_F(  JointStateBroadcasterTest,   DynamicJointStatePublishTest )
          {
           SetUpStateBroadcaster(   );
          
           test_published_dynamic_joint_state_message(  "dynamic_joint_states" );
          }
          
          TEST_F(  JointStateBroadcasterTest,   DynamicJointStatePublishTestLocalTopic )
          {
           SetUpStateBroadcaster(   );
           state_broadcaster_->get_node(   )->set_parameter(  {"use_local_topics",   true} );
          
           test_published_dynamic_joint_state_message(  "joint_state_broadcaster/dynamic_joint_states" );
          }
          
          TEST_F(  JointStateBroadcasterTest,   ExtraJointStatePublishTest )
          {
           // publishers not initialized yet
           ASSERT_FALSE(  state_broadcaster_->realtime_joint_state_publisher_ );
           ASSERT_FALSE(  state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
          
           SetUpStateBroadcaster(   );
          
           // Add extra joints as parameters
           const std::vector<std::string> extra_joint_names = {"extra1",   "extra2",   "extra3"};
           state_broadcaster_->get_node(   )->set_parameter(  {"extra_joints",   extra_joint_names} );
          
           // configure ok
           ASSERT_EQ(  state_broadcaster_->on_configure(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           ASSERT_EQ(  state_broadcaster_->on_activate(  rclcpp_lifecycle::State(   ) ),   NODE_SUCCESS );
          
           std::vector<std::string> all_joint_names = joint_names_;
           all_joint_names.insert(  all_joint_names.end(   ),   extra_joint_names.begin(   ),   extra_joint_names.end(   ) );
           const size_t NUM_JOINTS = all_joint_names.size(   );
          
           // joint state initialized
           const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
           ASSERT_THAT(  joint_state_msg.name,   ElementsAreArray(  all_joint_names ) );
           ASSERT_THAT(  joint_state_msg.position,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.velocity,   SizeIs(  NUM_JOINTS ) );
           ASSERT_THAT(  joint_state_msg.effort,   SizeIs(  NUM_JOINTS ) );
          
           // dynamic joint state initialized
           const auto & dynamic_joint_state_msg =
           state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
           ASSERT_THAT(  dynamic_joint_state_msg.joint_names,   SizeIs(  NUM_JOINTS ) );
          }

ros2_controllers/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_JOINT_STATE_BROADCASTER_HPP_
          #define TEST_JOINT_STATE_BROADCASTER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "joint_state_broadcaster/joint_state_broadcaster.hpp"
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          
          using hardware_interface::HW_IF_EFFORT;
          using hardware_interface::HW_IF_POSITION;
          using hardware_interface::HW_IF_VELOCITY;
          
          // subclassing and friending so we can access member variables
      33  class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster
          {
      35   FRIEND_TEST(  JointStateBroadcasterTest,   ConfigureErrorTest );
      36   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTest );
      37   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestWithoutJointsParameter );
      38   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestWithoutInterfacesParameter );
      39   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestTwoJointsOneInterface );
      40   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestOneJointTwoInterfaces );
      41   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestTwoJointTwoInterfacesAllMissing );
      42   FRIEND_TEST(  JointStateBroadcasterTest,   ActivateTestTwoJointTwoInterfacesOneMissing );
      43   FRIEND_TEST(  JointStateBroadcasterTest,   TestCustomInterfaceWithoutMapping );
      44   FRIEND_TEST(  JointStateBroadcasterTest,   TestCustomInterfaceMapping );
      45   FRIEND_TEST(  JointStateBroadcasterTest,   TestCustomInterfaceMappingUpdate );
      46   FRIEND_TEST(  JointStateBroadcasterTest,   ExtraJointStatePublishTest );
          };
          
      49  class JointStateBroadcasterTest : public ::testing::Test
          {
          public:
      52   static void SetUpTestCase(   );
      53   static void TearDownTestCase(   );
          
      55   void SetUp(   );
      56   void TearDown(   );
          
      58   void SetUpStateBroadcaster(  
           const std::vector<std::string> & joint_names = {},  
           const std::vector<std::string> & interfaces = {} );
          
      62   void init_broadcaster_and_set_parameters(  
           const std::vector<std::string> & joint_names = {},  
           const std::vector<std::string> & interfaces = {} );
          
      66   void assign_state_interfaces(  
           const std::vector<std::string> & joint_names = {},  
           const std::vector<std::string> & interfaces = {} );
          
      70   void test_published_joint_state_message(  const std::string & topic );
          
      72   void test_published_dynamic_joint_state_message(  const std::string & topic );
          
          protected:
           // dummy joint state values used for tests
      76   const std::vector<std::string> joint_names_ = {"joint1",   "joint2",   "joint3"};
      77   const std::vector<std::string> interface_names_ = {HW_IF_POSITION,   HW_IF_VELOCITY,   HW_IF_EFFORT};
      78   std::string custom_interface_name_ = "measured_position";
      79   std::vector<double> joint_values_ = {1.1,   2.1,   3.1};
           double custom_joint_value_ = 3.5;
          
           hardware_interface::StateInterface joint_1_pos_state_{
           joint_names_[0],   interface_names_[0],   &joint_values_[0]};
           hardware_interface::StateInterface joint_2_pos_state_{
      85   joint_names_[1],   interface_names_[0],   &joint_values_[1]};
           hardware_interface::StateInterface joint_3_pos_state_{
           joint_names_[2],   interface_names_[0],   &joint_values_[2]};
           hardware_interface::StateInterface joint_1_vel_state_{
           joint_names_[0],   interface_names_[1],   &joint_values_[0]};
           hardware_interface::StateInterface joint_2_vel_state_{
           joint_names_[1],   interface_names_[1],   &joint_values_[1]};
           hardware_interface::StateInterface joint_3_vel_state_{
           joint_names_[2],   interface_names_[1],   &joint_values_[2]};
           hardware_interface::StateInterface joint_1_eff_state_{
           joint_names_[0],   interface_names_[2],   &joint_values_[0]};
           hardware_interface::StateInterface joint_2_eff_state_{
           joint_names_[1],   interface_names_[2],   &joint_values_[1]};
           hardware_interface::StateInterface joint_3_eff_state_{
           joint_names_[2],   interface_names_[2],   &joint_values_[2]};
          
           hardware_interface::StateInterface joint_X_custom_state{
           joint_names_[0],   custom_interface_name_,   &custom_joint_value_};
          
           std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
          };
          
          #endif // TEST_JOINT_STATE_BROADCASTER_HPP_

ros2_controllers/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      25  TEST(  TestLoadJointStateBroadcaster,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_joint_state_broadcaster",   "joint_state_broadcaster/JointStateBroadcaster" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp

          // Copyright (  c ) 2022 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
          #define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
          
          #include <rclcpp/rclcpp.hpp>
          #include <string>
          #include <unordered_map>
          
          namespace joint_trajectory_controller
          {
          static const rclcpp::Logger LOGGER =
           rclcpp::get_logger(  "joint_trajectory_controller.interpolation_methods" );
          
          namespace interpolation_methods
          {
      29  enum class InterpolationMethod
          {
           NONE,  
           VARIABLE_DEGREE_SPLINE
          };
          
          const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE;
          
      37  const std::unordered_map<InterpolationMethod,   std::string> InterpolationMethodMap(  
           {{InterpolationMethod::NONE,   "none"},   {InterpolationMethod::VARIABLE_DEGREE_SPLINE,   "splines"}} );
          
      40  [[nodiscard]] inline InterpolationMethod from_string(  const std::string & interpolation_method )
          {
           if (  interpolation_method.compare(  InterpolationMethodMap.at(  InterpolationMethod::NONE ) ) == 0 )
           {
           return InterpolationMethod::NONE;
           }
           else if (  
           !interpolation_method.compare(  
           InterpolationMethodMap.at(  InterpolationMethod::VARIABLE_DEGREE_SPLINE ) ) == 0 )
           {
           return InterpolationMethod::VARIABLE_DEGREE_SPLINE;
           }
           // Default
           else
           {
           RCLCPP_INFO_STREAM(  
           LOGGER,  
           "No interpolation method parameter was given. Using the default,   VARIABLE_DEGREE_SPLINE." );
           return InterpolationMethod::VARIABLE_DEGREE_SPLINE;
           }
          }
          } // namespace interpolation_methods
          } // namespace joint_trajectory_controller
          
          #endif // JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_

ros2_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

       1  // Copyright (  c ) 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
          #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
          
          #include <chrono>
          #include <memory>
          #include <mutex>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "control_msgs/action/follow_joint_trajectory.hpp"
          #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
          #include "control_toolbox/pid.hpp"
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "joint_trajectory_controller/interpolation_methods.hpp"
          #include "joint_trajectory_controller/tolerances.hpp"
          #include "joint_trajectory_controller/visibility_control.h"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/subscription.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp/timer.hpp"
          #include "rclcpp_action/server.hpp"
          #include "rclcpp_action/types.hpp"
          #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "realtime_tools/realtime_buffer.h"
          #include "realtime_tools/realtime_publisher.h"
          #include "realtime_tools/realtime_server_goal_handle.h"
          #include "trajectory_msgs/msg/joint_trajectory.hpp"
          #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
          
          using namespace std::chrono_literals; // NOLINT
          
          namespace rclcpp_action
          {
          template <typename ActionT>
      52  class ServerGoalHandle;
          } // namespace rclcpp_action
          namespace rclcpp_lifecycle
          {
      56  class State;
          } // namespace rclcpp_lifecycle
          
      59  namespace joint_trajectory_controller
          {
          class Trajectory;
          
          class JointTrajectoryController : public controller_interface::ControllerInterface
          {
          public:
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           JointTrajectoryController(   );
          
           /**
           * @brief command_interface_configuration This controller requires the position command
           * interfaces for the controlled joints
           */
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration command_interface_configuration(   ) const override;
          
           /**
           * @brief command_interface_configuration This controller requires the position and velocity
           * state interfaces for the controlled joints
           */
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::InterfaceConfiguration state_interface_configuration(   ) const override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::return_type update(  
           const rclcpp::Time & time,   const rclcpp::Duration & period ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_init(   ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_activate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_cleanup(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_error(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           controller_interface::CallbackReturn on_shutdown(  
           const rclcpp_lifecycle::State & previous_state ) override;
          
          protected:
           std::vector<std::string> joint_names_;
           std::vector<std::string> command_interface_types_;
           std::vector<std::string> state_interface_types_;
          
           // To reduce number of variables and to make the code shorter the interfaces are ordered in types
           // as the following constants
           const std::vector<std::string> allowed_interface_types_ = {
           hardware_interface::HW_IF_POSITION,  
           hardware_interface::HW_IF_VELOCITY,  
           hardware_interface::HW_IF_ACCELERATION,  
           hardware_interface::HW_IF_EFFORT,  
           };
          
           // Preallocate variables used in the realtime update(   ) function
           trajectory_msgs::msg::JointTrajectoryPoint state_current_;
           trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
           trajectory_msgs::msg::JointTrajectoryPoint state_error_;
          
           // Degrees of freedom
           size_t dof_;
          
           // Parameters for some special cases,   e.g. hydraulics powered robots
           // Run the controller in open-loop,   i.e.,   read hardware states only when starting controller.
           // This is useful when robot is not exactly following the commanded trajectory.
           bool open_loop_control_ = false;
           trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
           /// Allow integration in goal trajectories to accept goals without position or velocity specified
           bool allow_integration_in_goal_trajectories_ = false;
           /// Specify interpolation method. Default to splines.
           interpolation_methods::InterpolationMethod interpolation_method_{
           interpolation_methods::DEFAULT_INTERPOLATION};
          
           double state_publish_rate_;
           double action_monitor_rate_;
          
           // The interfaces are defined as the types in 'allowed_interface_types_' member.
           // For convenience,   for each type the interfaces are ordered so that i-th position
           // matches i-th index in joint_names_
           template <typename T>
           using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
          
           InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
           InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
          
           bool has_position_state_interface_ = false;
           bool has_velocity_state_interface_ = false;
           bool has_acceleration_state_interface_ = false;
           bool has_position_command_interface_ = false;
           bool has_velocity_command_interface_ = false;
           bool has_acceleration_command_interface_ = false;
           bool has_effort_command_interface_ = false;
          
           /// If true,   a velocity feedforward term plus corrective PID term is used
           bool use_closed_loop_pid_adapter_ = false;
           using PidPtr = std::shared_ptr<control_toolbox::Pid>;
           std::vector<PidPtr> pids_;
           // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
           std::vector<double> ff_velocity_scale_;
           // reserved storage for result of the command when closed loop pid adapter is used
           std::vector<double> tmp_command_;
          
           // TODO(  karsten1987 ): eventually activate and deactivate subscriber directly when its supported
           bool subscriber_is_active_ = false;
           rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
           nullptr;
          
           std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
           std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
           std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
           realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
           traj_msg_external_point_ptr_;
          
           using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
           using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
           using StatePublisherPtr = std::unique_ptr<StatePublisher>;
           rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_;
           StatePublisherPtr state_publisher_;
          
           rclcpp::Duration state_publisher_period_ = rclcpp::Duration(  20ms );
           rclcpp::Time last_state_publish_time_;
          
           using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
           using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
           using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
           using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
          
           rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
           bool allow_partial_joints_goal_ = false;
           RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal,   if any.
           rclcpp::TimerBase::SharedPtr goal_handle_timer_;
           rclcpp::Duration action_monitor_period_ = rclcpp::Duration(  50ms );
          
           // callbacks for action_server_
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           rclcpp_action::GoalResponse goal_callback(  
           const rclcpp_action::GoalUUID & uuid,   std::shared_ptr<const FollowJTrajAction::Goal> goal );
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           rclcpp_action::CancelResponse cancel_callback(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle );
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void feedback_setup_callback(  
           std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle );
          
           // fill trajectory_msg so it matches joints controlled by this controller
           // positions set to current position,   velocities,   accelerations and efforts to 0.0
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void fill_partial_goal(  
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg ) const;
           // sorts the joints of the incoming message to our local order
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void sort_to_local_joint_order(  
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg );
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool validate_trajectory_msg(  const trajectory_msgs::msg::JointTrajectory & trajectory ) const;
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void add_new_trajectory_msg(  
           const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg );
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool validate_trajectory_point_field(  
           size_t joint_names_size,   const std::vector<double> & vector_field,  
           const std::string & string_for_vector_field,   size_t i,   bool allow_empty ) const;
          
           SegmentTolerances default_tolerances_;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void preempt_active_goal(   );
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void set_hold_position(   );
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool reset(   );
          
           using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void publish_state(  
           const JointTrajectoryPoint & desired_state,   const JointTrajectoryPoint & current_state,  
           const JointTrajectoryPoint & state_error );
          
           void read_state_from_hardware(  JointTrajectoryPoint & state );
          
           bool read_state_from_command_interfaces(  JointTrajectoryPoint & state );
          
          private:
           bool contains_interface_type(  
           const std::vector<std::string> & interface_type_list,   const std::string & interface_type );
          
           void resize_joint_trajectory_point(  
           trajectory_msgs::msg::JointTrajectoryPoint & point,   size_t size );
          };
          
          } // namespace joint_trajectory_controller
          
          #endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_

ros2_controllers/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

       1  // Copyright 2013 PAL Robotics S.L.
          // All rights reserved.
          //
          // Software License Agreement (  BSD License 2.0 )
          // Redistribution and use in source and binary forms,   with or without
          // modification,   are permitted provided that the following conditions are met:
          // * Redistributions of source code must retain the above copyright notice,  
          // this list of conditions and the following disclaimer.
          // * Redistributions in binary form must reproduce the above copyright
          // notice,   this list of conditions and the following disclaimer in the
          // documentation and/or other materials provided with the distribution.
          // * Neither the name of PAL Robotics S.L. nor the names of its
          // contributors may be used to endorse or promote products derived from
          // this software without specific prior written permission.
          //
          // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
          // AND ANY EXPRESS OR IMPLIED WARRANTIES,   INCLUDING,   BUT NOT LIMITED TO,   THE
          // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
          // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
          // LIABLE FOR ANY DIRECT,   INDIRECT,   INCIDENTAL,   SPECIAL,   EXEMPLARY,   OR
          // CONSEQUENTIAL DAMAGES (  INCLUDING,   BUT NOT LIMITED TO,   PROCUREMENT OF
          // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,   DATA,   OR PROFITS; OR BUSINESS
          // INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,   WHETHER IN
          // CONTRACT,   STRICT LIABILITY,   OR TORT (  INCLUDING NEGLIGENCE OR OTHERWISE )
          // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,   EVEN IF ADVISED OF THE
          // POSSIBILITY OF SUCH DAMAGE.
          
          /// \author Adolfo Rodriguez Tsouroukdissian
          
          #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
          #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
          
          #include <cassert>
          #include <cmath>
          #include <string>
          #include <vector>
          
          #include "control_msgs/action/follow_joint_trajectory.hpp"
          
          #include "rclcpp/node.hpp"
          
          namespace joint_trajectory_controller
          {
          /**
           * \brief Trajectory state tolerances for position,   velocity and acceleration variables.
           *
           * A tolerance value of zero means that no tolerance will be applied for that variable.
           */
          struct StateTolerances
          {
           double position = 0.0;
           double velocity = 0.0;
           double acceleration = 0.0;
          };
          
          /**
           * \brief Trajectory segment tolerances.
           */
          struct SegmentTolerances
          {
           explicit SegmentTolerances(  size_t size = 0 ) : state_tolerance(  size ),   goal_state_tolerance(  size ) {}
          
           /** State tolerances that apply during segment execution. */
           std::vector<StateTolerances> state_tolerance;
          
           /** State tolerances that apply for the goal state only.*/
           std::vector<StateTolerances> goal_state_tolerance;
          
           /** Extra time after the segment end time allowed to reach the goal state tolerances. */
           double goal_time_tolerance = 0.0;
          };
          
          /**
           * \brief Populate trajectory segment tolerances using data from the ROS node.
           *
           * It is assumed that the following parameter structure is followed on the provided LifecycleNode.
           * Unspecified parameters will take the defaults shown in the comments:
           *
           * \code
           * constraints:
           * goal_time: 1.0 # Defaults to zero
           * stopped_velocity_tolerance: 0.02 # Defaults to 0.01
           * foo_joint:
           * trajectory: 0.05 # Defaults to zero (  ie. the tolerance is not enforced )
           * goal: 0.03 # Defaults to zero (  ie. the tolerance is not enforced )
           * bar_joint:
           * goal: 0.01
           * \endcode
           *
           * \param node LifecycleNode where the tolerances are specified.
           * \param joint_names Names of joints to look for in the parameter server for a tolerance specification.
           * \return Trajectory segment tolerances.
           */
      94  SegmentTolerances get_segment_tolerances(  
           const rclcpp_lifecycle::LifecycleNode & node,   const std::vector<std::string> & joint_names )
          {
           const auto n_joints = joint_names.size(   );
           SegmentTolerances tolerances;
          
           // State and goal state tolerances
           double stopped_velocity_tolerance = 0.01;
           node.get_parameter_or<double>(  
           "constraints.stopped_velocity_tolerance",   stopped_velocity_tolerance,  
           stopped_velocity_tolerance );
          
           tolerances.state_tolerance.resize(  n_joints );
           tolerances.goal_state_tolerance.resize(  n_joints );
           for (  size_t i = 0; i < n_joints; ++i )
           {
           const std::string prefix = "constraints." + joint_names[i];
          
           node.get_parameter_or<double>(  
           prefix + ".trajectory",   tolerances.state_tolerance[i].position,   0.0 );
           node.get_parameter_or<double>(  
           prefix + ".goal",   tolerances.goal_state_tolerance[i].position,   0.0 );
          
           auto logger = rclcpp::get_logger(  "tolerance" );
           RCLCPP_DEBUG(  
           logger,   "%s %f",   (  prefix + ".trajectory" ).c_str(   ),   tolerances.state_tolerance[i].position );
           RCLCPP_DEBUG(  
           logger,   "%s %f",   (  prefix + ".goal" ).c_str(   ),   tolerances.goal_state_tolerance[i].position );
          
           tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance;
           }
          
           // Goal time tolerance
           node.get_parameter_or<double>(  "constraints.goal_time",   tolerances.goal_time_tolerance,   0.0 );
          
           return tolerances;
          }
          
          /**
           * \param state_error State error to check.
           * \param joint_idx Joint index for the state error
           * \param state_tolerance State tolerance of joint to check \p state_error against.
           * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true
           * \return True if \p state_error fulfills \p state_tolerance.
           */
     139  inline bool check_state_tolerance_per_joint(  
           const trajectory_msgs::msg::JointTrajectoryPoint & state_error,   int joint_idx,  
           const StateTolerances & state_tolerance,   bool show_errors = false )
          {
           using std::abs;
           const double error_position = state_error.positions[joint_idx];
           const double error_velocity =
           state_error.velocities.empty(   ) ? 0.0 : state_error.velocities[joint_idx];
           const double error_acceleration =
           state_error.accelerations.empty(   ) ? 0.0 : state_error.accelerations[joint_idx];
          
           const bool is_valid =
           !(  state_tolerance.position > 0.0 && abs(  error_position ) > state_tolerance.position ) &&
           !(  state_tolerance.velocity > 0.0 && abs(  error_velocity ) > state_tolerance.velocity ) &&
           !(  state_tolerance.acceleration > 0.0 && abs(  error_acceleration ) > state_tolerance.acceleration );
          
           if (  is_valid )
           {
           return true;
           }
          
           if (  show_errors )
           {
           const auto logger = rclcpp::get_logger(  "tolerances" );
           RCLCPP_ERROR(  logger,   "Path state tolerances failed:" );
          
           if (  state_tolerance.position > 0.0 && abs(  error_position ) > state_tolerance.position )
           {
           RCLCPP_ERROR(  
           logger,   "Position Error: %f,   Position Tolerance: %f",   error_position,  
           state_tolerance.position );
           }
           if (  state_tolerance.velocity > 0.0 && abs(  error_velocity ) > state_tolerance.velocity )
           {
           RCLCPP_ERROR(  
           logger,   "Velocity Error: %f,   Velocity Tolerance: %f",   error_velocity,  
           state_tolerance.velocity );
           }
           if (  
           state_tolerance.acceleration > 0.0 && abs(  error_acceleration ) > state_tolerance.acceleration )
           {
           RCLCPP_ERROR(  
           logger,   "Acceleration Error: %f,   Acceleration Tolerance: %f",   error_acceleration,  
           state_tolerance.acceleration );
           }
           }
           return false;
          }
          
          } // namespace joint_trajectory_controller
          
          #endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_

ros2_controllers/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

          // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
          #define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
          
          #include <memory>
          #include <vector>
          
          #include "joint_trajectory_controller/interpolation_methods.hpp"
          #include "joint_trajectory_controller/visibility_control.h"
          #include "rclcpp/time.hpp"
          #include "trajectory_msgs/msg/joint_trajectory.hpp"
          #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
          namespace joint_trajectory_controller
          {
          using TrajectoryPointIter = std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
          using TrajectoryPointConstIter =
           std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
          
      32  class Trajectory
          {
          public:
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
      36   Trajectory(   );
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           explicit Trajectory(  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           explicit Trajectory(  
           const rclcpp::Time & current_time,  
           const trajectory_msgs::msg::JointTrajectoryPoint & current_point,  
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
          
           /// Set the point before the trajectory message is replaced/appended
           /// Example: if we receive a new trajectory message and it's first point is 0.5 seconds
           /// from the current one,   we call this function to log the current state,   then
           /// append/replace the current trajectory
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void set_point_before_trajectory_msg(  
           const rclcpp::Time & current_time,  
           const trajectory_msgs::msg::JointTrajectoryPoint & current_point );
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void update(  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
          
           /// Find the segment (  made up of 2 points ) and its expected state from the
           /// containing trajectory.
           /**
           * Sampling trajectory at given \p sample_time.
           * If position in the \p end_segment_itr is missing it will be deduced from provided velocity,   or acceleration respectively.
           * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment.
           *
           * Specific case returns for start_segment_itr and end_segment_itr:
           * - Sampling before the trajectory start:
           * start_segment_itr = begin(   ),   end_segment_itr = begin(   )
           * - Sampling exactly on a point of the trajectory:
           * start_segment_itr = iterator where point is,   end_segment_itr = iterator after start_segment_itr
           * - Sampling between points:
           * start_segment_itr = iterator before the sampled point,   end_segment_itr = iterator after start_segment_itr
           * - Sampling after entire trajectory:
           * start_segment_itr = --end(   ),   end_segment_itr = end(   )
           * - Sampling empty msg or before the time given in set_point_before_trajectory_msg(   )
           * return false
           *
           * \param[in] sample_time Time at which trajectory will be sampled.
           * \param[in] interpolation_method Specify whether splines,   another method,   or no interpolation at all.
           * \param[out] expected_state Calculated new at \p sample_time.
           * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above.
           * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
           */
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool sample(  
           const rclcpp::Time & sample_time,  
           const interpolation_methods::InterpolationMethod interpolation_method,  
           trajectory_msgs::msg::JointTrajectoryPoint & output_state,  
           TrajectoryPointConstIter & start_segment_itr,   TrajectoryPointConstIter & end_segment_itr );
          
           /**
           * Do interpolation between 2 states given a time in between their respective timestamps
           *
           * The start and end states need not necessarily be specified all the way to the acceleration level:
           * - If only \b positions are specified,   linear interpolation will be used.
           * - If \b positions and \b velocities are specified,   a cubic spline will be used.
           * - If \b positions,   \b velocities and \b accelerations are specified,   a quintic spline will be used.
           *
           * If start and end states have different specifications
           * (  eg. start is position-only,   end is position-velocity ),   the lowest common specification will be used
           * (  position-only in the example ).
           *
           * \param[in] time_a Time at which the segment state equals \p state_a.
           * \param[in] state_a State at \p time_a.
           * \param[in] time_b Time at which the segment state equals \p state_b.
           * \param[in] state_b State at time \p time_b.
           * \param[in] sample_time The time to sample,   between time_a and time_b.
           * \param[out] output The state at \p sample_time.
           */
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           void interpolate_between_points(  
           const rclcpp::Time & time_a,   const trajectory_msgs::msg::JointTrajectoryPoint & state_a,  
           const rclcpp::Time & time_b,   const trajectory_msgs::msg::JointTrajectoryPoint & state_b,  
           const rclcpp::Time & sample_time,   trajectory_msgs::msg::JointTrajectoryPoint & output );
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           TrajectoryPointConstIter begin(   ) const;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           TrajectoryPointConstIter end(   ) const;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           rclcpp::Time time_from_start(   ) const;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool has_trajectory_msg(   ) const;
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg(   ) const
           {
           return trajectory_msg_;
           }
          
           JOINT_TRAJECTORY_CONTROLLER_PUBLIC
     135   rclcpp::Time get_trajectory_start_time(   ) const { return trajectory_start_time_; }
          
     137   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
           bool is_sampled_already(   ) const { return sampled_already_; }
          
          private:
     141   void deduce_from_derivatives(  
           trajectory_msgs::msg::JointTrajectoryPoint & first_state,  
           trajectory_msgs::msg::JointTrajectoryPoint & second_state,   const size_t dim,  
           const double delta_t );
          
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
           rclcpp::Time trajectory_start_time_;
          
           rclcpp::Time time_before_traj_msg_;
           trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
          
           bool sampled_already_ = false;
          };
          
          /**
           * \return The map between \p t1 indices (  implicitly encoded in return vector indices ) to \p t2 indices.
           * If \p t1 is <tt>"{C,   B}"</tt> and \p t2 is <tt>"{A,   B,   C,   D}"</tt>,   the associated mapping vector is
           * <tt>"{2,   1}"</tt>.
           */
          template <class T>
          inline std::vector<size_t> mapping(  const T & t1,   const T & t2 )
          {
           // t1 must be a subset of t2
           if (  t1.size(   ) > t2.size(   ) )
           {
           return std::vector<size_t>(   );
           }
          
           std::vector<size_t> mapping_vector(  t1.size(   ) ); // Return value
           for (  auto t1_it = t1.begin(   ); t1_it != t1.end(   ); ++t1_it )
           {
           auto t2_it = std::find(  t2.begin(   ),   t2.end(   ),   *t1_it );
           if (  t2.end(   ) == t2_it )
           {
           return std::vector<size_t>(   );
           }
           else
           {
           const size_t t1_dist = std::distance(  t1.begin(   ),   t1_it );
           const size_t t2_dist = std::distance(  t2.begin(   ),   t2_it );
           mapping_vector[t1_dist] = t2_dist;
           }
           }
           return mapping_vector;
          }
          
          } // namespace joint_trajectory_controller
          
          #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_

ros2_controllers/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_
          #define JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__(  (  dllexport ) )
          #define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(  dllexport )
          #define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(  dllimport )
          #endif
          #ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT
          #else
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT
          #endif
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC
          #define JOINT_TRAJECTORY_CONTROLLER_LOCAL
          #else
          #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define JOINT_TRAJECTORY_CONTROLLER_IMPORT
          #if __GNUC__ >= 4
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC
          #define JOINT_TRAJECTORY_CONTROLLER_LOCAL
          #endif
          #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE
          #endif
          
          #endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_

ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

       1  // Copyright (  c ) 2021 ros2_control Development Team
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
          
          #include <stddef.h>
          #include <chrono>
          #include <functional>
          #include <memory>
          #include <ostream>
          #include <ratio>
          #include <string>
          #include <vector>
          
          #include "angles/angles.h"
          #include "builtin_interfaces/msg/duration.hpp"
          #include "builtin_interfaces/msg/time.hpp"
          #include "controller_interface/helpers.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "joint_trajectory_controller/trajectory.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/parameter.hpp"
          #include "rclcpp/qos.hpp"
          #include "rclcpp/qos_event.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp_action/create_server.hpp"
          #include "rclcpp_action/server_goal_handle.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "std_msgs/msg/header.hpp"
          
          namespace joint_trajectory_controller
          {
      48  JointTrajectoryController::JointTrajectoryController(   )
          : controller_interface::ControllerInterface(   ),   joint_names_(  {} ),   dof_(  0 )
          {
          }
          
      53  controller_interface::CallbackReturn JointTrajectoryController::on_init(   )
          {
           try
           {
           // with the lifecycle node being initialized,   we can declare parameters
           joint_names_ = auto_declare<std::vector<std::string>>(  "joints",   joint_names_ );
           command_interface_types_ =
           auto_declare<std::vector<std::string>>(  "command_interfaces",   command_interface_types_ );
           state_interface_types_ =
           auto_declare<std::vector<std::string>>(  "state_interfaces",   state_interface_types_ );
           allow_partial_joints_goal_ =
           auto_declare<bool>(  "allow_partial_joints_goal",   allow_partial_joints_goal_ );
           open_loop_control_ = auto_declare<bool>(  "open_loop_control",   open_loop_control_ );
           allow_integration_in_goal_trajectories_ = auto_declare<bool>(  
           "allow_integration_in_goal_trajectories",   allow_integration_in_goal_trajectories_ );
           state_publish_rate_ = auto_declare<double>(  "state_publish_rate",   50.0 );
           action_monitor_rate_ = auto_declare<double>(  "action_monitor_rate",   20.0 );
          
           std::string interpolation_string = auto_declare<std::string>(  
           "interpolation_method",   interpolation_methods::InterpolationMethodMap.at(  
           interpolation_methods::DEFAULT_INTERPOLATION ) );
           interpolation_method_ = interpolation_methods::from_string(  interpolation_string );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           return CallbackReturn::SUCCESS;
          }
          
          controller_interface::InterfaceConfiguration
      86  JointTrajectoryController::command_interface_configuration(   ) const
          {
           controller_interface::InterfaceConfiguration conf;
           conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           if (  dof_ == 0 )
           {
           fprintf(  
           stderr,  
           "During ros2_control interface configuration,   degrees of freedom is not valid;"
           " it should be positive. Actual DOF is %zu\n",  
           dof_ );
           std::exit(  EXIT_FAILURE );
           }
           conf.names.reserve(  dof_ * command_interface_types_.size(   ) );
           for (  const auto & joint_name : joint_names_ )
           {
           for (  const auto & interface_type : command_interface_types_ )
           {
           conf.names.push_back(  joint_name + "/" + interface_type );
           }
           }
           return conf;
          }
          
          controller_interface::InterfaceConfiguration
     111  JointTrajectoryController::state_interface_configuration(   ) const
          {
           controller_interface::InterfaceConfiguration conf;
           conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
           conf.names.reserve(  dof_ * state_interface_types_.size(   ) );
           for (  const auto & joint_name : joint_names_ )
           {
           for (  const auto & interface_type : state_interface_types_ )
           {
           conf.names.push_back(  joint_name + "/" + interface_type );
           }
           }
           return conf;
          }
          
     126  controller_interface::return_type JointTrajectoryController::update(  
     127   const rclcpp::Time & time,   const rclcpp::Duration & period )
          {
           if (  get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           return controller_interface::return_type::OK;
           }
          
           auto compute_error_for_joint = [&](  
           JointTrajectoryPoint & error,   int index,  
           const JointTrajectoryPoint & current,  
           const JointTrajectoryPoint & desired ) {
           // error defined as the difference between current and desired
           error.positions[index] =
           angles::shortest_angular_distance(  current.positions[index],   desired.positions[index] );
           if (  has_velocity_state_interface_ && has_velocity_command_interface_ )
           {
           error.velocities[index] = desired.velocities[index] - current.velocities[index];
           }
           if (  has_acceleration_state_interface_ && has_acceleration_command_interface_ )
           {
           error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
           }
           };
          
           // Check if a new external message has been received from nonRT threads
           auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(   );
           auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(   );
           if (  current_external_msg != *new_external_msg )
           {
           fill_partial_goal(  *new_external_msg );
           sort_to_local_joint_order(  *new_external_msg );
           // TODO(  denis ): Add here integration of position and velocity
           traj_external_point_ptr_->update(  *new_external_msg );
           }
          
           // TODO(  anyone ): can I here also use const on joint_interface since the reference_wrapper is not
           // changed,   but its value only?
           auto assign_interface_from_point =
           [&](  auto & joint_interface,   const std::vector<double> & trajectory_point_interface ) {
           for (  size_t index = 0; index < dof_; ++index )
           {
           joint_interface[index].get(   ).set_value(  trajectory_point_interface[index] );
           }
           };
          
           // current state update
           state_current_.time_from_start.set__sec(  0 );
           read_state_from_hardware(  state_current_ );
          
           // currently carrying out a trajectory
           if (  traj_point_active_ptr_ && (  *traj_point_active_ptr_ )->has_trajectory_msg(   ) )
           {
           bool first_sample = false;
           // if sampling the first time,   set the point before you sample
           if (  !(  *traj_point_active_ptr_ )->is_sampled_already(   ) )
           {
           first_sample = true;
           if (  open_loop_control_ )
           {
           (  *traj_point_active_ptr_ )->set_point_before_trajectory_msg(  time,   last_commanded_state_ );
           }
           else
           {
           (  *traj_point_active_ptr_ )->set_point_before_trajectory_msg(  time,   state_current_ );
           }
           }
          
           // find segment for current timestamp
           TrajectoryPointConstIter start_segment_itr,   end_segment_itr;
           const bool valid_point =
           (  *traj_point_active_ptr_ )
           ->sample(  time,   interpolation_method_,   state_desired_,   start_segment_itr,   end_segment_itr );
          
           if (  valid_point )
           {
           bool tolerance_violated_while_moving = false;
           bool outside_goal_tolerance = false;
           bool within_goal_time = true;
           double time_difference = 0.0;
           const bool before_last_point = end_segment_itr != (  *traj_point_active_ptr_ )->end(   );
          
           // Check state/goal tolerance
           for (  size_t index = 0; index < dof_; ++index )
           {
           compute_error_for_joint(  state_error_,   index,   state_current_,   state_desired_ );
          
           // Always check the state tolerance on the first sample in case the first sample
           // is the last point
           if (  
           (  before_last_point || first_sample ) &&
           !check_state_tolerance_per_joint(  
           state_error_,   index,   default_tolerances_.state_tolerance[index],   false ) )
           {
           tolerance_violated_while_moving = true;
           }
           // past the final point,   check that we end up inside goal tolerance
           if (  
           !before_last_point &&
           !check_state_tolerance_per_joint(  
           state_error_,   index,   default_tolerances_.goal_state_tolerance[index],   false ) )
           {
           outside_goal_tolerance = true;
          
           if (  default_tolerances_.goal_time_tolerance != 0.0 )
           {
           // if we exceed goal_time_tolerance set it to aborted
           const rclcpp::Time traj_start = (  *traj_point_active_ptr_ )->get_trajectory_start_time(   );
           const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
          
           time_difference = get_node(   )->now(   ).seconds(   ) - traj_end.seconds(   );
          
           if (  time_difference > default_tolerances_.goal_time_tolerance )
           {
           within_goal_time = false;
           }
           }
           }
           }
          
           // set values for next hardware write(   ) if tolerance is met
           if (  !tolerance_violated_while_moving && within_goal_time )
           {
           if (  use_closed_loop_pid_adapter_ )
           {
           // Update PIDs
           for (  auto i = 0ul; i < dof_; ++i )
           {
           tmp_command_[i] = (  state_desired_.velocities[i] * ff_velocity_scale_[i] ) +
           pids_[i]->computeCommand(  
           state_desired_.positions[i] - state_current_.positions[i],  
           state_desired_.velocities[i] - state_current_.velocities[i],  
           (  uint64_t )period.nanoseconds(   ) );
           }
           }
          
           // set values for next hardware write(   )
           if (  has_position_command_interface_ )
           {
           assign_interface_from_point(  joint_command_interface_[0],   state_desired_.positions );
           }
           if (  has_velocity_command_interface_ )
           {
           if (  use_closed_loop_pid_adapter_ )
           {
           assign_interface_from_point(  joint_command_interface_[1],   tmp_command_ );
           }
           else
           {
           assign_interface_from_point(  joint_command_interface_[1],   state_desired_.velocities );
           }
           }
           if (  has_acceleration_command_interface_ )
           {
           assign_interface_from_point(  joint_command_interface_[2],   state_desired_.accelerations );
           }
           if (  has_effort_command_interface_ )
           {
           if (  use_closed_loop_pid_adapter_ )
           {
           assign_interface_from_point(  joint_command_interface_[3],   tmp_command_ );
           }
           else
           {
           assign_interface_from_point(  joint_command_interface_[3],   state_desired_.effort );
           }
           }
          
           // store the previous command. Used in open-loop control mode
           last_commanded_state_ = state_desired_;
           }
          
           const auto active_goal = *rt_active_goal_.readFromRT(   );
           if (  active_goal )
           {
           // send feedback
           auto feedback = std::make_shared<FollowJTrajAction::Feedback>(   );
           feedback->header.stamp = time;
           feedback->joint_names = joint_names_;
          
           feedback->actual = state_current_;
           feedback->desired = state_desired_;
           feedback->error = state_error_;
           active_goal->setFeedback(  feedback );
          
           // check abort
           if (  tolerance_violated_while_moving )
           {
           set_hold_position(   );
           auto result = std::make_shared<FollowJTrajAction::Result>(   );
          
           RCLCPP_WARN(  get_node(   )->get_logger(   ),   "Aborted due to state tolerance violation" );
           result->set__error_code(  FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED );
           active_goal->setAborted(  result );
           // TODO(  matthew-reynolds ): Need a lock-free write here
           // See https://github.com/ros-controls/ros2_controllers/issues/168
           rt_active_goal_.writeFromNonRT(  RealtimeGoalHandlePtr(   ) );
          
           // check goal tolerance
           }
           else if (  !before_last_point )
           {
           if (  !outside_goal_tolerance )
           {
           auto res = std::make_shared<FollowJTrajAction::Result>(   );
           res->set__error_code(  FollowJTrajAction::Result::SUCCESSFUL );
           active_goal->setSucceeded(  res );
           // TODO(  matthew-reynolds ): Need a lock-free write here
           // See https://github.com/ros-controls/ros2_controllers/issues/168
           rt_active_goal_.writeFromNonRT(  RealtimeGoalHandlePtr(   ) );
          
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Goal reached,   success!" );
           }
           else if (  !within_goal_time )
           {
           set_hold_position(   );
           auto result = std::make_shared<FollowJTrajAction::Result>(   );
           result->set__error_code(  FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED );
           active_goal->setAborted(  result );
           // TODO(  matthew-reynolds ): Need a lock-free write here
           // See https://github.com/ros-controls/ros2_controllers/issues/168
           rt_active_goal_.writeFromNonRT(  RealtimeGoalHandlePtr(   ) );
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),   "Aborted due goal_time_tolerance exceeding by %f seconds",  
           time_difference );
           }
           // else,   run another cycle while waiting for outside_goal_tolerance
           // to be satisfied or violated within the goal_time_tolerance
           }
           }
           }
           }
          
           publish_state(  state_desired_,   state_current_,   state_error_ );
           return controller_interface::return_type::OK;
          }
          
     363  void JointTrajectoryController::read_state_from_hardware(  JointTrajectoryPoint & state )
          {
           auto assign_point_from_interface =
           [&](  std::vector<double> & trajectory_point_interface,   const auto & joint_interface ) {
           for (  size_t index = 0; index < dof_; ++index )
           {
           trajectory_point_interface[index] = joint_interface[index].get(   ).get_value(   );
           }
           };
          
           // Assign values from the hardware
           // Position states always exist
           assign_point_from_interface(  state.positions,   joint_state_interface_[0] );
           // velocity and acceleration states are optional
           if (  has_velocity_state_interface_ )
           {
           assign_point_from_interface(  state.velocities,   joint_state_interface_[1] );
           // Acceleration is used only in combination with velocity
           if (  has_acceleration_state_interface_ )
           {
           assign_point_from_interface(  state.accelerations,   joint_state_interface_[2] );
           }
           else
           {
           // Make empty so the property is ignored during interpolation
           state.accelerations.clear(   );
           }
           }
           else
           {
           // Make empty so the property is ignored during interpolation
           state.velocities.clear(   );
           state.accelerations.clear(   );
           }
          }
          
     399  bool JointTrajectoryController::read_state_from_command_interfaces(  JointTrajectoryPoint & state )
          {
           bool has_values = true;
          
           auto assign_point_from_interface =
           [&](  std::vector<double> & trajectory_point_interface,   const auto & joint_interface ) {
           for (  size_t index = 0; index < dof_; ++index )
           {
           trajectory_point_interface[index] = joint_interface[index].get(   ).get_value(   );
           }
           };
          
           auto interface_has_values = [](  const auto & joint_interface ) {
           return std::find_if(  joint_interface.begin(   ),   joint_interface.end(   ),   [](  const auto & interface ) {
           return std::isnan(  interface.get(   ).get_value(   ) );
           } ) == joint_interface.end(   );
           };
          
           // Assign values from the command interfaces as state. Therefore needs check for both.
           // Position state interface has to exist always
           if (  has_position_command_interface_ && interface_has_values(  joint_command_interface_[0] ) )
           {
           assign_point_from_interface(  state.positions,   joint_command_interface_[0] );
           }
           else
           {
           state.positions.clear(   );
           has_values = false;
           }
           // velocity and acceleration states are optional
           if (  has_velocity_state_interface_ )
           {
           if (  has_velocity_command_interface_ && interface_has_values(  joint_command_interface_[1] ) )
           {
           assign_point_from_interface(  state.velocities,   joint_command_interface_[1] );
           }
           else
           {
           state.velocities.clear(   );
           has_values = false;
           }
           }
           else
           {
           state.velocities.clear(   );
           }
           // Acceleration is used only in combination with velocity
           if (  has_acceleration_state_interface_ )
           {
           if (  has_acceleration_command_interface_ && interface_has_values(  joint_command_interface_[2] ) )
           {
           assign_point_from_interface(  state.accelerations,   joint_command_interface_[2] );
           }
           else
           {
           state.accelerations.clear(   );
           has_values = false;
           }
           }
           else
           {
           state.accelerations.clear(   );
           }
          
           return has_values;
          }
          
     466  controller_interface::CallbackReturn JointTrajectoryController::on_configure(  
     467   const rclcpp_lifecycle::State & )
          {
           const auto logger = get_node(   )->get_logger(   );
          
           // update parameters
           joint_names_ = get_node(   )->get_parameter(  "joints" ).as_string_array(   );
           if (  (  dof_ > 0 ) && (  joint_names_.size(   ) != dof_ ) )
           {
           RCLCPP_ERROR(  
           logger,  
           "The JointTrajectoryController does not support restarting with a different number of DOF" );
           // TODO(  andyz ): update vector lengths if num. joints did change and re-initialize them so we
           // can continue
           return CallbackReturn::FAILURE;
           }
          
           dof_ = joint_names_.size(   );
          
           if (  !reset(   ) )
           {
           return CallbackReturn::FAILURE;
           }
          
           if (  joint_names_.empty(   ) )
           {
           RCLCPP_WARN(  logger,   "'joints' parameter is empty." );
           }
          
           // Specialized,   child controllers set interfaces before calling configure function.
           if (  command_interface_types_.empty(   ) )
           {
           command_interface_types_ = get_node(   )->get_parameter(  "command_interfaces" ).as_string_array(   );
           }
          
           if (  command_interface_types_.empty(   ) )
           {
           RCLCPP_ERROR(  logger,   "'command_interfaces' parameter is empty." );
           return CallbackReturn::FAILURE;
           }
          
           // Check if only allowed interface types are used and initialize storage to avoid memory
           // allocation during activation
           joint_command_interface_.resize(  allowed_interface_types_.size(   ) );
           for (  const auto & interface : command_interface_types_ )
           {
           auto it =
           std::find(  allowed_interface_types_.begin(   ),   allowed_interface_types_.end(   ),   interface );
           if (  it == allowed_interface_types_.end(   ) )
           {
           RCLCPP_ERROR(  logger,   "Command interface type '%s' not allowed!",   interface.c_str(   ) );
           return CallbackReturn::FAILURE;
           }
           }
          
           // Check if command interfaces combination is valid. Valid combinations are:
           // 1. effort
           // 2. velocity
           // 2. position [velocity,   [acceleration]]
          
           has_position_command_interface_ =
           contains_interface_type(  command_interface_types_,   hardware_interface::HW_IF_POSITION );
           has_velocity_command_interface_ =
           contains_interface_type(  command_interface_types_,   hardware_interface::HW_IF_VELOCITY );
           has_acceleration_command_interface_ =
           contains_interface_type(  command_interface_types_,   hardware_interface::HW_IF_ACCELERATION );
           has_effort_command_interface_ =
           contains_interface_type(  command_interface_types_,   hardware_interface::HW_IF_EFFORT );
          
           if (  has_velocity_command_interface_ )
           {
           // if there is only velocity then use also PID adapter
           if (  command_interface_types_.size(   ) == 1 )
           {
           use_closed_loop_pid_adapter_ = true;
           }
           else if (  !has_position_command_interface_ )
           {
           RCLCPP_ERROR(  
           logger,  
           "'velocity' command interface can be used either alone or 'position' "
           "interface has to be present." );
           return CallbackReturn::FAILURE;
           }
           // invalid: acceleration is defined and no velocity
           }
           else if (  has_acceleration_command_interface_ )
           {
           RCLCPP_ERROR(  
           logger,  
           "'acceleration' command interface can only be used if 'velocity' and "
           "'position' interfaces are present" );
           return CallbackReturn::FAILURE;
           }
          
           // effort can't be combined with other interfaces
           if (  has_effort_command_interface_ )
           {
           if (  command_interface_types_.size(   ) == 1 )
           {
           use_closed_loop_pid_adapter_ = true;
           }
           else
           {
           RCLCPP_ERROR(  logger,   "'effort' command interface has to be used alone." );
           return CallbackReturn::FAILURE;
           }
           }
          
           if (  use_closed_loop_pid_adapter_ )
           {
           pids_.resize(  dof_ );
           ff_velocity_scale_.resize(  dof_ );
           tmp_command_.resize(  dof_,   0.0 );
          
           // Init PID gains from ROS parameter server
           for (  size_t i = 0; i < pids_.size(   ); ++i )
           {
           const std::string prefix = "gains." + joint_names_[i];
           const auto k_p = auto_declare<double>(  prefix + ".p",   0.0 );
           const auto k_i = auto_declare<double>(  prefix + ".i",   0.0 );
           const auto k_d = auto_declare<double>(  prefix + ".d",   0.0 );
           const auto i_clamp = auto_declare<double>(  prefix + ".i_clamp",   0.0 );
           ff_velocity_scale_[i] = auto_declare<double>(  "ff_velocity_scale/" + joint_names_[i],   0.0 );
           // Initialize PID
           pids_[i] = std::make_shared<control_toolbox::Pid>(  k_p,   k_i,   k_d,   i_clamp,   -i_clamp );
           }
           }
          
           // Read always state interfaces from the parameter because they can be used
           // independently from the controller's type.
           // Specialized,   child controllers should set its default value.
           state_interface_types_ = get_node(   )->get_parameter(  "state_interfaces" ).as_string_array(   );
          
           if (  state_interface_types_.empty(   ) )
           {
           RCLCPP_ERROR(  logger,   "'state_interfaces' parameter is empty." );
           return CallbackReturn::FAILURE;
           }
          
           if (  contains_interface_type(  state_interface_types_,   hardware_interface::HW_IF_EFFORT ) )
           {
           RCLCPP_ERROR(  logger,   "State interface type 'effort' not allowed!" );
           return CallbackReturn::FAILURE;
           }
          
           // Check if only allowed interface types are used and initialize storage to avoid memory
           // allocation during activation
           // Note: 'effort' storage is also here,   but never used. Still,   for this is OK.
           joint_state_interface_.resize(  allowed_interface_types_.size(   ) );
           for (  const auto & interface : state_interface_types_ )
           {
           auto it =
           std::find(  allowed_interface_types_.begin(   ),   allowed_interface_types_.end(   ),   interface );
           if (  it == allowed_interface_types_.end(   ) )
           {
           RCLCPP_ERROR(  logger,   "State interface type '%s' not allowed!",   interface.c_str(   ) );
           return CallbackReturn::FAILURE;
           }
           }
          
           has_position_state_interface_ =
           contains_interface_type(  state_interface_types_,   hardware_interface::HW_IF_POSITION );
           has_velocity_state_interface_ =
           contains_interface_type(  state_interface_types_,   hardware_interface::HW_IF_VELOCITY );
           has_acceleration_state_interface_ =
           contains_interface_type(  state_interface_types_,   hardware_interface::HW_IF_ACCELERATION );
          
           if (  has_velocity_state_interface_ )
           {
           if (  !has_position_state_interface_ )
           {
           RCLCPP_ERROR(  
           logger,  
           "'velocity' state interface cannot be used if 'position' interface "
           "is missing." );
           return CallbackReturn::FAILURE;
           }
           }
           else
           {
           if (  has_acceleration_state_interface_ )
           {
           RCLCPP_ERROR(  
           logger,  
           "'acceleration' state interface cannot be used if 'position' and 'velocity' "
           "interfaces are not present." );
           return CallbackReturn::FAILURE;
           }
           if (  has_velocity_command_interface_ && command_interface_types_.size(   ) == 1 )
           {
           RCLCPP_ERROR(  
           logger,  
           "'velocity' command interface can only be used alone if 'velocity' and "
           "'position' state interfaces are present" );
           return CallbackReturn::FAILURE;
           }
           // effort is always used alone so no need for size check
           if (  has_effort_command_interface_ )
           {
           RCLCPP_ERROR(  
           logger,  
           "'effort' command interface can only be used alone if 'velocity' and "
           "'position' state interfaces are present" );
           return CallbackReturn::FAILURE;
           }
           }
          
           auto get_interface_list = [](  const std::vector<std::string> & interface_types ) {
           std::stringstream ss_interfaces;
           for (  size_t index = 0; index < interface_types.size(   ); ++index )
           {
           if (  index != 0 )
           {
           ss_interfaces << " ";
           }
           ss_interfaces << interface_types[index];
           }
           return ss_interfaces.str(   );
           };
          
           // Print output so users can be sure the interface setup is correct
           RCLCPP_INFO(  
           logger,   "Command interfaces are [%s] and state interfaces are [%s].",  
           get_interface_list(  command_interface_types_ ).c_str(   ),  
           get_interface_list(  state_interface_types_ ).c_str(   ) );
          
           default_tolerances_ = get_segment_tolerances(  *get_node(   ),   joint_names_ );
          
           // Read parameters customizing controller for special cases
           open_loop_control_ = get_node(   )->get_parameter(  "open_loop_control" ).get_value<bool>(   );
           allow_integration_in_goal_trajectories_ =
           get_node(   )->get_parameter(  "allow_integration_in_goal_trajectories" ).get_value<bool>(   );
          
           // subscriber callback
           // non realtime
           // TODO(  karsten ): check if traj msg and point time are valid
           auto callback = [this](  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg ) -> void {
           if (  !validate_trajectory_msg(  *msg ) )
           {
           return;
           }
          
           // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
           // always replace old msg with new one for now
           if (  subscriber_is_active_ )
           {
           add_new_trajectory_msg(  msg );
           }
           };
          
           // TODO(  karsten1987 ): create subscriber with subscription deactivated
           joint_command_subscriber_ =
           get_node(   )->create_subscription<trajectory_msgs::msg::JointTrajectory>(  
           "~/joint_trajectory",   rclcpp::SystemDefaultsQoS(   ),   callback );
          
           // TODO(  karsten1987 ): no lifecycle for subscriber yet
           // joint_command_subscriber_->on_activate(   );
          
           // State publisher
           RCLCPP_INFO(  logger,   "Controller state will be published at %.2f Hz.",   state_publish_rate_ );
           if (  state_publish_rate_ > 0.0 )
           {
           state_publisher_period_ = rclcpp::Duration::from_seconds(  1.0 / state_publish_rate_ );
           }
           else
           {
           state_publisher_period_ = rclcpp::Duration::from_seconds(  0.0 );
           }
          
           publisher_ =
           get_node(   )->create_publisher<ControllerStateMsg>(  "~/state",   rclcpp::SystemDefaultsQoS(   ) );
           state_publisher_ = std::make_unique<StatePublisher>(  publisher_ );
          
           state_publisher_->lock(   );
           state_publisher_->msg_.joint_names = joint_names_;
           state_publisher_->msg_.desired.positions.resize(  dof_ );
           state_publisher_->msg_.desired.velocities.resize(  dof_ );
           state_publisher_->msg_.desired.accelerations.resize(  dof_ );
           state_publisher_->msg_.actual.positions.resize(  dof_ );
           state_publisher_->msg_.error.positions.resize(  dof_ );
           if (  has_velocity_state_interface_ )
           {
           state_publisher_->msg_.actual.velocities.resize(  dof_ );
           state_publisher_->msg_.error.velocities.resize(  dof_ );
           }
           if (  has_acceleration_state_interface_ )
           {
           state_publisher_->msg_.actual.accelerations.resize(  dof_ );
           state_publisher_->msg_.error.accelerations.resize(  dof_ );
           }
           state_publisher_->unlock(   );
          
           last_state_publish_time_ = get_node(   )->now(   );
          
           // action server configuration
           allow_partial_joints_goal_ =
           get_node(   )->get_parameter(  "allow_partial_joints_goal" ).get_value<bool>(   );
           if (  allow_partial_joints_goal_ )
           {
           RCLCPP_INFO(  logger,   "Goals with partial set of joints are allowed" );
           }
          
           RCLCPP_INFO(  logger,   "Action status changes will be monitored at %.2f Hz.",   action_monitor_rate_ );
           action_monitor_period_ = rclcpp::Duration::from_seconds(  1.0 / action_monitor_rate_ );
          
           using namespace std::placeholders;
           action_server_ = rclcpp_action::create_server<FollowJTrajAction>(  
           get_node(   )->get_node_base_interface(   ),   get_node(   )->get_node_clock_interface(   ),  
           get_node(   )->get_node_logging_interface(   ),   get_node(   )->get_node_waitables_interface(   ),  
           std::string(  get_node(   )->get_name(   ) ) + "/follow_joint_trajectory",  
           std::bind(  &JointTrajectoryController::goal_callback,   this,   _1,   _2 ),  
           std::bind(  &JointTrajectoryController::cancel_callback,   this,   _1 ),  
           std::bind(  &JointTrajectoryController::feedback_setup_callback,   this,   _1 ) );
          
           resize_joint_trajectory_point(  state_current_,   dof_ );
           resize_joint_trajectory_point(  state_desired_,   dof_ );
           resize_joint_trajectory_point(  state_error_,   dof_ );
           resize_joint_trajectory_point(  last_commanded_state_,   dof_ );
          
           return CallbackReturn::SUCCESS;
          }
          
     789  controller_interface::CallbackReturn JointTrajectoryController::on_activate(  
     790   const rclcpp_lifecycle::State & )
          {
           // order all joints in the storage
           for (  const auto & interface : command_interface_types_ )
           {
           auto it =
           std::find(  allowed_interface_types_.begin(   ),   allowed_interface_types_.end(   ),   interface );
           auto index = std::distance(  allowed_interface_types_.begin(   ),   it );
           if (  !controller_interface::get_ordered_interfaces(  
           command_interfaces_,   joint_names_,   interface,   joint_command_interface_[index] ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Expected %zu '%s' command interfaces,   got %zu.",   dof_,  
           interface.c_str(   ),   joint_command_interface_[index].size(   ) );
           return CallbackReturn::ERROR;
           }
           }
           for (  const auto & interface : state_interface_types_ )
           {
           auto it =
           std::find(  allowed_interface_types_.begin(   ),   allowed_interface_types_.end(   ),   interface );
           auto index = std::distance(  allowed_interface_types_.begin(   ),   it );
           if (  !controller_interface::get_ordered_interfaces(  
           state_interfaces_,   joint_names_,   interface,   joint_state_interface_[index] ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Expected %zu '%s' state interfaces,   got %zu.",   dof_,  
           interface.c_str(   ),   joint_state_interface_[index].size(   ) );
           return CallbackReturn::ERROR;
           }
           }
          
           // Store 'home' pose
           traj_msg_home_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           traj_msg_home_ptr_->header.stamp.sec = 0;
           traj_msg_home_ptr_->header.stamp.nanosec = 0;
           traj_msg_home_ptr_->points.resize(  1 );
           traj_msg_home_ptr_->points[0].time_from_start.sec = 0;
           traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000;
           traj_msg_home_ptr_->points[0].positions.resize(  joint_state_interface_[0].size(   ) );
           for (  size_t index = 0; index < joint_state_interface_[0].size(   ); ++index )
           {
           traj_msg_home_ptr_->points[0].positions[index] =
           joint_state_interface_[0][index].get(   ).get_value(   );
           }
          
           traj_external_point_ptr_ = std::make_shared<Trajectory>(   );
           traj_home_point_ptr_ = std::make_shared<Trajectory>(   );
           traj_msg_external_point_ptr_.writeFromNonRT(  
           std::shared_ptr<trajectory_msgs::msg::JointTrajectory>(   ) );
          
           subscriber_is_active_ = true;
           traj_point_active_ptr_ = &traj_external_point_ptr_;
           last_state_publish_time_ = get_node(   )->now(   );
          
           // Initialize current state storage if hardware state has tracking offset
           read_state_from_hardware(  state_current_ );
           read_state_from_hardware(  state_desired_ );
           read_state_from_hardware(  last_commanded_state_ );
           // Handle restart of controller by reading from commands if
           // those are not nan
           trajectory_msgs::msg::JointTrajectoryPoint state;
           resize_joint_trajectory_point(  state,   dof_ );
           if (  read_state_from_command_interfaces(  state ) )
           {
           state_current_ = state;
           state_desired_ = state;
           last_commanded_state_ = state;
           }
          
           // TODO(  karsten1987 ): activate subscriptions of subscriber
           return CallbackReturn::SUCCESS;
          }
          
     864  controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(  
     865   const rclcpp_lifecycle::State & )
          {
           // TODO(  anyone ): How to halt when using effort commands?
           for (  size_t index = 0; index < dof_; ++index )
           {
           if (  has_position_command_interface_ )
           {
           joint_command_interface_[0][index].get(   ).set_value(  
           joint_command_interface_[0][index].get(   ).get_value(   ) );
           }
          
           if (  has_velocity_command_interface_ )
           {
           joint_command_interface_[1][index].get(   ).set_value(  0.0 );
           }
          
           if (  has_effort_command_interface_ )
           {
           joint_command_interface_[3][index].get(   ).set_value(  0.0 );
           }
           }
          
           for (  size_t index = 0; index < allowed_interface_types_.size(   ); ++index )
           {
           joint_command_interface_[index].clear(   );
           joint_state_interface_[index].clear(   );
           }
           release_interfaces(   );
          
           subscriber_is_active_ = false;
          
           return CallbackReturn::SUCCESS;
          }
          
     899  controller_interface::CallbackReturn JointTrajectoryController::on_cleanup(  
     900   const rclcpp_lifecycle::State & )
          {
           // go home
           traj_home_point_ptr_->update(  traj_msg_home_ptr_ );
           traj_point_active_ptr_ = &traj_home_point_ptr_;
          
           return CallbackReturn::SUCCESS;
          }
          
     909  controller_interface::CallbackReturn JointTrajectoryController::on_error(  
     910   const rclcpp_lifecycle::State & )
          {
           if (  !reset(   ) )
           {
           return CallbackReturn::ERROR;
           }
           return CallbackReturn::SUCCESS;
          }
          
     919  bool JointTrajectoryController::reset(   )
          {
           subscriber_is_active_ = false;
           joint_command_subscriber_.reset(   );
          
           for (  const auto & pid : pids_ )
           {
           pid->reset(   );
           }
          
           // iterator has no default value
           // prev_traj_point_ptr_;
           traj_point_active_ptr_ = nullptr;
           traj_external_point_ptr_.reset(   );
           traj_home_point_ptr_.reset(   );
           traj_msg_home_ptr_.reset(   );
          
           // reset pids
           for (  const auto & pid : pids_ )
           {
           pid->reset(   );
           }
          
           return true;
          }
          
     945  controller_interface::CallbackReturn JointTrajectoryController::on_shutdown(  
     946   const rclcpp_lifecycle::State & )
          {
           // TODO(  karsten1987 ): what to do?
          
           return CallbackReturn::SUCCESS;
          }
          
     953  void JointTrajectoryController::publish_state(  
     954   const JointTrajectoryPoint & desired_state,   const JointTrajectoryPoint & current_state,  
     955   const JointTrajectoryPoint & state_error )
          {
           if (  state_publisher_period_.seconds(   ) <= 0.0 )
           {
           return;
           }
          
           if (  get_node(   )->now(   ) < (  last_state_publish_time_ + state_publisher_period_ ) )
           {
           return;
           }
          
           if (  state_publisher_ && state_publisher_->trylock(   ) )
           {
           last_state_publish_time_ = get_node(   )->now(   );
           state_publisher_->msg_.header.stamp = last_state_publish_time_;
           state_publisher_->msg_.desired.positions = desired_state.positions;
           state_publisher_->msg_.desired.velocities = desired_state.velocities;
           state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
           state_publisher_->msg_.actual.positions = current_state.positions;
           state_publisher_->msg_.error.positions = state_error.positions;
           if (  has_velocity_state_interface_ )
           {
           state_publisher_->msg_.actual.velocities = current_state.velocities;
           state_publisher_->msg_.error.velocities = state_error.velocities;
           }
           if (  has_acceleration_state_interface_ )
           {
           state_publisher_->msg_.actual.accelerations = current_state.accelerations;
           state_publisher_->msg_.error.accelerations = state_error.accelerations;
           }
          
           state_publisher_->unlockAndPublish(   );
           }
          }
          
     991  rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(  
     992   const rclcpp_action::GoalUUID &,   std::shared_ptr<const FollowJTrajAction::Goal> goal )
          {
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Received new action goal" );
          
           // Precondition: Running controller
           if (  get_state(   ).id(   ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Can't accept new action goals. Controller is not running." );
           return rclcpp_action::GoalResponse::REJECT;
           }
          
           if (  !validate_trajectory_msg(  goal->trajectory ) )
           {
           return rclcpp_action::GoalResponse::REJECT;
           }
          
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Accepted new action goal" );
           return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
          }
          
    1013  rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(  
           const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle )
          {
           RCLCPP_INFO(  get_node(   )->get_logger(   ),   "Got request to cancel goal" );
          
           // Check that cancel request refers to currently active goal (  if any )
           const auto active_goal = *rt_active_goal_.readFromNonRT(   );
           if (  active_goal && active_goal->gh_ == goal_handle )
           {
           // Controller uptime
           // Enter hold current position mode
           set_hold_position(   );
          
           RCLCPP_DEBUG(  
           get_node(   )->get_logger(   ),   "Canceling active action goal because cancel callback received." );
          
           // Mark the current goal as canceled
           auto action_res = std::make_shared<FollowJTrajAction::Result>(   );
           active_goal->setCanceled(  action_res );
           rt_active_goal_.writeFromNonRT(  RealtimeGoalHandlePtr(   ) );
           }
           return rclcpp_action::CancelResponse::ACCEPT;
          }
          
    1037  void JointTrajectoryController::feedback_setup_callback(  
           std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle )
          {
           // Update new trajectory
           {
           preempt_active_goal(   );
           auto traj_msg =
           std::make_shared<trajectory_msgs::msg::JointTrajectory>(  goal_handle->get_goal(   )->trajectory );
          
           add_new_trajectory_msg(  traj_msg );
           }
          
           // Update the active goal
           RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(  goal_handle );
           rt_goal->preallocated_feedback_->joint_names = joint_names_;
           rt_goal->execute(   );
           rt_active_goal_.writeFromNonRT(  rt_goal );
          
           // Setup goal status checking timer
           goal_handle_timer_ = get_node(   )->create_wall_timer(  
           action_monitor_period_.to_chrono<std::chrono::seconds>(   ),  
           std::bind(  &RealtimeGoalHandle::runNonRealtime,   rt_goal ) );
          }
          
    1061  void JointTrajectoryController::fill_partial_goal(  
    1062   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg ) const
          {
           // joint names in the goal are a subset of existing joints,   as checked in goal_callback
           // so if the size matches,   the goal contains all controller joints
           if (  dof_ == trajectory_msg->joint_names.size(   ) )
           {
           return;
           }
          
           trajectory_msg->joint_names.reserve(  dof_ );
          
           for (  size_t index = 0; index < dof_; ++index )
           {
           {
           if (  
           std::find(  
           trajectory_msg->joint_names.begin(   ),   trajectory_msg->joint_names.end(   ),  
           joint_names_[index] ) != trajectory_msg->joint_names.end(   ) )
           {
           // joint found on msg
           continue;
           }
           trajectory_msg->joint_names.push_back(  joint_names_[index] );
          
           for (  auto & it : trajectory_msg->points )
           {
           // Assume hold position with 0 velocity and acceleration for missing joints
           if (  !it.positions.empty(   ) )
           {
           if (  
           has_position_command_interface_ &&
           !std::isnan(  joint_command_interface_[0][index].get(   ).get_value(   ) ) )
           {
           // copy last command if cmd interface exists
           it.positions.push_back(  joint_command_interface_[0][index].get(   ).get_value(   ) );
           }
           else if (  has_position_state_interface_ )
           {
           // copy current state if state interface exists
           it.positions.push_back(  joint_state_interface_[0][index].get(   ).get_value(   ) );
           }
           }
           if (  !it.velocities.empty(   ) )
           {
           it.velocities.push_back(  0.0 );
           }
           if (  !it.accelerations.empty(   ) )
           {
           it.accelerations.push_back(  0.0 );
           }
           if (  !it.effort.empty(   ) )
           {
           it.effort.push_back(  0.0 );
           }
           }
           }
           }
          }
          
    1121  void JointTrajectoryController::sort_to_local_joint_order(  
    1122   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg )
          {
           // rearrange all points in the trajectory message based on mapping
           std::vector<size_t> mapping_vector = mapping(  trajectory_msg->joint_names,   joint_names_ );
           auto remap = [this](  
           const std::vector<double> & to_remap,  
           const std::vector<size_t> & mapping ) -> std::vector<double> {
           if (  to_remap.empty(   ) )
           {
           return to_remap;
           }
           if (  to_remap.size(   ) != mapping.size(   ) )
           {
           RCLCPP_WARN(  
           get_node(   )->get_logger(   ),   "Invalid input size (  %zu ) for sorting",   to_remap.size(   ) );
           return to_remap;
           }
           std::vector<double> output;
           output.resize(  mapping.size(   ),   0.0 );
           for (  size_t index = 0; index < mapping.size(   ); ++index )
           {
           auto map_index = mapping[index];
           output[map_index] = to_remap[index];
           }
           return output;
           };
          
           for (  size_t index = 0; index < trajectory_msg->points.size(   ); ++index )
           {
           trajectory_msg->points[index].positions =
           remap(  trajectory_msg->points[index].positions,   mapping_vector );
          
           trajectory_msg->points[index].velocities =
           remap(  trajectory_msg->points[index].velocities,   mapping_vector );
          
           trajectory_msg->points[index].accelerations =
           remap(  trajectory_msg->points[index].accelerations,   mapping_vector );
          
           trajectory_msg->points[index].effort =
           remap(  trajectory_msg->points[index].effort,   mapping_vector );
           }
          }
          
    1165  bool JointTrajectoryController::validate_trajectory_point_field(  
    1166   size_t joint_names_size,   const std::vector<double> & vector_field,  
    1167   const std::string & string_for_vector_field,   size_t i,   bool allow_empty ) const
          {
           if (  allow_empty && vector_field.empty(   ) )
           {
           return true;
           }
           if (  joint_names_size != vector_field.size(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Mismatch between joint_names (  %zu ) and %s (  %zu ) at point #%zu.",  
           joint_names_size,   string_for_vector_field.c_str(   ),   vector_field.size(   ),   i );
           return false;
           }
           return true;
          }
          
    1183  bool JointTrajectoryController::validate_trajectory_msg(  
    1184   const trajectory_msgs::msg::JointTrajectory & trajectory ) const
          {
           // If partial joints goals are not allowed,   goal should specify all controller joints
           if (  !allow_partial_joints_goal_ )
           {
           if (  trajectory.joint_names.size(   ) != dof_ )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "Joints on incoming trajectory don't match the controller joints." );
           return false;
           }
           }
          
           if (  trajectory.joint_names.empty(   ) )
           {
           RCLCPP_ERROR(  get_node(   )->get_logger(   ),   "Empty joint names on incoming trajectory." );
           return false;
           }
          
           const auto trajectory_start_time = static_cast<rclcpp::Time>(  trajectory.header.stamp );
           // If the starting time it set to 0.0,   it means the controller should start it now.
           // Otherwise we check if the trajectory ends before the current time,  
           // in which case it can be ignored.
           if (  trajectory_start_time.seconds(   ) != 0.0 )
           {
           auto trajectory_end_time = trajectory_start_time;
           for (  const auto & p : trajectory.points )
           {
           trajectory_end_time += p.time_from_start;
           }
           if (  trajectory_end_time < get_node(   )->now(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "Received trajectory with non zero time start time (  %f ) that ends on the past (  %f )",  
           trajectory_start_time.seconds(   ),   trajectory_end_time.seconds(   ) );
           return false;
           }
           }
          
           for (  size_t i = 0; i < trajectory.joint_names.size(   ); ++i )
           {
           const std::string & incoming_joint_name = trajectory.joint_names[i];
          
           auto it = std::find(  joint_names_.begin(   ),   joint_names_.end(   ),   incoming_joint_name );
           if (  it == joint_names_.end(   ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),   "Incoming joint %s doesn't match the controller's joints.",  
           incoming_joint_name.c_str(   ) );
           return false;
           }
           }
          
           rclcpp::Duration previous_traj_time(  0ms );
           for (  size_t i = 0; i < trajectory.points.size(   ); ++i )
           {
           if (  (  i > 0 ) && (  rclcpp::Duration(  trajectory.points[i].time_from_start ) <= previous_traj_time ) )
           {
           RCLCPP_ERROR(  
           get_node(   )->get_logger(   ),  
           "Time between points %zu and %zu is not strictly increasing,   it is %f and %f respectively",  
           i - 1,   i,   previous_traj_time.seconds(   ),  
           rclcpp::Duration(  trajectory.points[i].time_from_start ).seconds(   ) );
           return false;
           }
           previous_traj_time = trajectory.points[i].time_from_start;
          
           const size_t joint_count = trajectory.joint_names.size(   );
           const auto & points = trajectory.points;
           // This currently supports only position,   velocity and acceleration inputs
           if (  allow_integration_in_goal_trajectories_ )
           {
           const bool all_empty = points[i].positions.empty(   ) && points[i].velocities.empty(   ) &&
           points[i].accelerations.empty(   );
           const bool position_error =
           !points[i].positions.empty(   ) &&
           !validate_trajectory_point_field(  joint_count,   points[i].positions,   "positions",   i,   false );
           const bool velocity_error =
           !points[i].velocities.empty(   ) &&
           !validate_trajectory_point_field(  joint_count,   points[i].velocities,   "velocities",   i,   false );
           const bool acceleration_error =
           !points[i].accelerations.empty(   ) &&
           !validate_trajectory_point_field(  
           joint_count,   points[i].accelerations,   "accelerations",   i,   false );
           if (  all_empty || position_error || velocity_error || acceleration_error )
           {
           return false;
           }
           }
           else if (  
           !validate_trajectory_point_field(  joint_count,   points[i].positions,   "positions",   i,   false ) ||
           !validate_trajectory_point_field(  joint_count,   points[i].velocities,   "velocities",   i,   true ) ||
           !validate_trajectory_point_field(  
           joint_count,   points[i].accelerations,   "accelerations",   i,   true ) ||
           !validate_trajectory_point_field(  joint_count,   points[i].effort,   "effort",   i,   true ) )
           {
           return false;
           }
           }
           return true;
          }
          
    1288  void JointTrajectoryController::add_new_trajectory_msg(  
    1289   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg )
          {
           traj_msg_external_point_ptr_.writeFromNonRT(  traj_msg );
          }
          
    1294  void JointTrajectoryController::preempt_active_goal(   )
          {
           const auto active_goal = *rt_active_goal_.readFromNonRT(   );
           if (  active_goal )
           {
           set_hold_position(   );
           auto action_res = std::make_shared<FollowJTrajAction::Result>(   );
           action_res->set__error_code(  FollowJTrajAction::Result::INVALID_GOAL );
           action_res->set__error_string(  "Current goal cancelled due to new incoming action." );
           active_goal->setCanceled(  action_res );
           rt_active_goal_.writeFromNonRT(  RealtimeGoalHandlePtr(   ) );
           }
          }
          
    1308  void JointTrajectoryController::set_hold_position(   )
          {
           trajectory_msgs::msg::JointTrajectory empty_msg;
           empty_msg.header.stamp = rclcpp::Time(  0 );
          
           auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(  empty_msg );
           add_new_trajectory_msg(  traj_msg );
          }
          
    1317  bool JointTrajectoryController::contains_interface_type(  
    1318   const std::vector<std::string> & interface_type_list,   const std::string & interface_type )
          {
           return std::find(  interface_type_list.begin(   ),   interface_type_list.end(   ),   interface_type ) !=
           interface_type_list.end(   );
          }
          
    1324  void JointTrajectoryController::resize_joint_trajectory_point(  
    1325   trajectory_msgs::msg::JointTrajectoryPoint & point,   size_t size )
          {
           point.positions.resize(  size,   0.0 );
           if (  has_velocity_state_interface_ )
           {
           point.velocities.resize(  size,   0.0 );
           }
           if (  has_acceleration_state_interface_ )
           {
           point.accelerations.resize(  size,   0.0 );
           }
          }
          
          } // namespace joint_trajectory_controller
          
          #include "pluginlib/class_list_macros.hpp"
          
    1342  PLUGINLIB_EXPORT_CLASS(  
           joint_trajectory_controller::JointTrajectoryController,   controller_interface::ControllerInterface )

ros2_controllers/joint_trajectory_controller/src/trajectory.cpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include "joint_trajectory_controller/trajectory.hpp"
          
          #include <memory>
          
          #include "hardware_interface/macros.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "rcppmath/clamp.hpp"
          #include "std_msgs/msg/header.hpp"
          namespace joint_trajectory_controller
          {
      26  Trajectory::Trajectory(   ) : trajectory_start_time_(  0 ),   time_before_traj_msg_(  0 ) {}
          
      28  Trajectory::Trajectory(  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
          : trajectory_msg_(  joint_trajectory ),  
           trajectory_start_time_(  static_cast<rclcpp::Time>(  joint_trajectory->header.stamp ) )
          {
          }
          
      34  Trajectory::Trajectory(  
      35   const rclcpp::Time & current_time,  
      36   const trajectory_msgs::msg::JointTrajectoryPoint & current_point,  
      37   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
          : trajectory_msg_(  joint_trajectory ),  
           trajectory_start_time_(  static_cast<rclcpp::Time>(  joint_trajectory->header.stamp ) )
          {
           set_point_before_trajectory_msg(  current_time,   current_point );
           update(  joint_trajectory );
          }
          
      45  void Trajectory::set_point_before_trajectory_msg(  
      46   const rclcpp::Time & current_time,  
      47   const trajectory_msgs::msg::JointTrajectoryPoint & current_point )
          {
           time_before_traj_msg_ = current_time;
           state_before_traj_msg_ = current_point;
          }
          
      53  void Trajectory::update(  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
          {
           trajectory_msg_ = joint_trajectory;
           trajectory_start_time_ = static_cast<rclcpp::Time>(  joint_trajectory->header.stamp );
           sampled_already_ = false;
          }
          
      60  bool Trajectory::sample(  
      61   const rclcpp::Time & sample_time,  
      62   const interpolation_methods::InterpolationMethod interpolation_method,  
      63   trajectory_msgs::msg::JointTrajectoryPoint & output_state,  
      64   TrajectoryPointConstIter & start_segment_itr,   TrajectoryPointConstIter & end_segment_itr )
          {
           THROW_ON_NULLPTR(  trajectory_msg_ )
           output_state = trajectory_msgs::msg::JointTrajectoryPoint(   );
          
           if (  trajectory_msg_->points.empty(   ) )
           {
           start_segment_itr = end(   );
           end_segment_itr = end(   );
           return false;
           }
          
           // first sampling of this trajectory
           if (  !sampled_already_ )
           {
           if (  trajectory_start_time_.seconds(   ) == 0.0 )
           {
           trajectory_start_time_ = sample_time;
           }
          
           sampled_already_ = true;
           }
          
           // sampling before the current point
           if (  sample_time < time_before_traj_msg_ )
           {
           return false;
           }
          
           auto & first_point_in_msg = trajectory_msg_->points[0];
           const rclcpp::Time first_point_timestamp =
           trajectory_start_time_ + first_point_in_msg.time_from_start;
          
           // current time hasn't reached traj time of the first point in the msg yet
           if (  sample_time < first_point_timestamp )
           {
           // If interpolation is disabled,   just forward the next waypoint
           if (  interpolation_method == interpolation_methods::InterpolationMethod::NONE )
           {
           output_state = state_before_traj_msg_;
           }
           else
           {
           // it changes points only if position and velocity do not exist,   but their derivatives
           deduce_from_derivatives(  
           state_before_traj_msg_,   first_point_in_msg,   state_before_traj_msg_.positions.size(   ),  
           (  first_point_timestamp - time_before_traj_msg_ ).seconds(   ) );
          
           interpolate_between_points(  
           time_before_traj_msg_,   state_before_traj_msg_,   first_point_timestamp,   first_point_in_msg,  
           sample_time,   output_state );
           }
           start_segment_itr = begin(   ); // no segments before the first
           end_segment_itr = begin(   );
           return true;
           }
          
           // time_from_start + trajectory time is the expected arrival time of trajectory
           const auto last_idx = trajectory_msg_->points.size(   ) - 1;
           for (  size_t i = 0; i < last_idx; ++i )
           {
           auto & point = trajectory_msg_->points[i];
           auto & next_point = trajectory_msg_->points[i + 1];
          
           const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start;
           const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start;
          
           if (  sample_time >= t0 && sample_time < t1 )
           {
           // If interpolation is disabled,   just forward the next waypoint
           if (  interpolation_method == interpolation_methods::InterpolationMethod::NONE )
           {
           output_state = next_point;
           }
           // Do interpolation
           else
           {
           // it changes points only if position and velocity do not exist,   but their derivatives
           deduce_from_derivatives(  
           point,   next_point,   state_before_traj_msg_.positions.size(   ),   (  t1 - t0 ).seconds(   ) );
          
           interpolate_between_points(  t0,   point,   t1,   next_point,   sample_time,   output_state );
           }
           start_segment_itr = begin(   ) + i;
           end_segment_itr = begin(   ) + (  i + 1 );
           return true;
           }
           }
          
           // whole animation has played out
           start_segment_itr = --end(   );
           end_segment_itr = end(   );
           output_state = (  *start_segment_itr );
           // the trajectories in msg may have empty velocities/accel,   so resize them
           if (  output_state.velocities.empty(   ) )
           {
           output_state.velocities.resize(  output_state.positions.size(   ),   0.0 );
           }
           if (  output_state.accelerations.empty(   ) )
           {
           output_state.accelerations.resize(  output_state.positions.size(   ),   0.0 );
           }
           return true;
          }
          
     169  void Trajectory::interpolate_between_points(  
     170   const rclcpp::Time & time_a,   const trajectory_msgs::msg::JointTrajectoryPoint & state_a,  
     171   const rclcpp::Time & time_b,   const trajectory_msgs::msg::JointTrajectoryPoint & state_b,  
     172   const rclcpp::Time & sample_time,   trajectory_msgs::msg::JointTrajectoryPoint & output )
          {
           rclcpp::Duration duration_so_far = sample_time - time_a;
           rclcpp::Duration duration_btwn_points = time_b - time_a;
          
           const size_t dim = state_a.positions.size(   );
           output.positions.resize(  dim,   0.0 );
           output.velocities.resize(  dim,   0.0 );
           output.accelerations.resize(  dim,   0.0 );
          
           auto generate_powers = [](  int n,   double x,   double * powers ) {
           powers[0] = 1.0;
           for (  int i = 1; i <= n; ++i )
           {
           powers[i] = powers[i - 1] * x;
           }
           };
          
           bool has_velocity = !state_a.velocities.empty(   ) && !state_b.velocities.empty(   );
           bool has_accel = !state_a.accelerations.empty(   ) && !state_b.accelerations.empty(   );
           if (  duration_so_far.seconds(   ) < 0.0 )
           {
           duration_so_far = rclcpp::Duration::from_seconds(  0.0 );
           has_velocity = has_accel = false;
           }
           if (  duration_so_far.seconds(   ) > duration_btwn_points.seconds(   ) )
           {
           duration_so_far = duration_btwn_points;
           has_velocity = has_accel = false;
           }
          
           double t[6];
           generate_powers(  5,   duration_so_far.seconds(   ),   t );
          
           if (  !has_velocity && !has_accel )
           {
           // do linear interpolation
           for (  size_t i = 0; i < dim; ++i )
           {
           double start_pos = state_a.positions[i];
           double end_pos = state_b.positions[i];
          
           double coefficients[2] = {0.0,   0.0};
           coefficients[0] = start_pos;
           if (  duration_btwn_points.seconds(   ) != 0.0 )
           {
           coefficients[1] = (  end_pos - start_pos ) / duration_btwn_points.seconds(   );
           }
          
           output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1];
           output.velocities[i] = t[0] * coefficients[1];
           }
           }
           else if (  has_velocity && !has_accel )
           {
           // do cubic interpolation
           double T[4];
           generate_powers(  3,   duration_btwn_points.seconds(   ),   T );
          
           for (  size_t i = 0; i < dim; ++i )
           {
           double start_pos = state_a.positions[i];
           double start_vel = state_a.velocities[i];
           double end_pos = state_b.positions[i];
           double end_vel = state_b.velocities[i];
          
           double coefficients[4] = {0.0,   0.0,   0.0,   0.0};
           coefficients[0] = start_pos;
           coefficients[1] = start_vel;
           if (  duration_btwn_points.seconds(   ) != 0.0 )
           {
           coefficients[2] =
           (  -3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1] ) / T[2];
           coefficients[3] =
           (  2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1] ) / T[3];
           }
          
           output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] +
           t[2] * coefficients[2] + t[3] * coefficients[3];
           output.velocities[i] =
           t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3];
           output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3];
           }
           }
           else if (  has_velocity && has_accel )
           {
           // do quintic interpolation
           double T[6];
           generate_powers(  5,   duration_btwn_points.seconds(   ),   T );
          
           for (  size_t i = 0; i < dim; ++i )
           {
           double start_pos = state_a.positions[i];
           double start_vel = state_a.velocities[i];
           double start_acc = state_a.accelerations[i];
           double end_pos = state_b.positions[i];
           double end_vel = state_b.velocities[i];
           double end_acc = state_b.accelerations[i];
          
           double coefficients[6] = {0.0,   0.0,   0.0,   0.0,   0.0,   0.0};
           coefficients[0] = start_pos;
           coefficients[1] = start_vel;
           coefficients[2] = 0.5 * start_acc;
           if (  duration_btwn_points.seconds(   ) != 0.0 )
           {
           coefficients[3] = (  -20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] +
           end_acc * T[2] - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1] ) /
           (  2.0 * T[3] );
           coefficients[4] = (  30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] -
           2.0 * end_acc * T[2] + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1] ) /
           (  2.0 * T[4] );
           coefficients[5] = (  -12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] -
           6.0 * start_vel * T[1] - 6.0 * end_vel * T[1] ) /
           (  2.0 * T[5] );
           }
          
           output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] +
           t[2] * coefficients[2] + t[3] * coefficients[3] +
           t[4] * coefficients[4] + t[5] * coefficients[5];
           output.velocities[i] = t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] +
           t[2] * 3.0 * coefficients[3] + t[3] * 4.0 * coefficients[4] +
           t[4] * 5.0 * coefficients[5];
           output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] +
           t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5];
           }
           }
          }
          
     300  void Trajectory::deduce_from_derivatives(  
     301   trajectory_msgs::msg::JointTrajectoryPoint & first_state,  
     302   trajectory_msgs::msg::JointTrajectoryPoint & second_state,   const size_t dim,   const double delta_t )
          {
           if (  second_state.positions.empty(   ) )
           {
           second_state.positions.resize(  dim );
           if (  first_state.velocities.empty(   ) )
           {
           first_state.velocities.resize(  dim,   0.0 );
           }
           if (  second_state.velocities.empty(   ) )
           {
           second_state.velocities.resize(  dim );
           if (  first_state.accelerations.empty(   ) )
           {
           first_state.accelerations.resize(  dim,   0.0 );
           }
           for (  size_t i = 0; i < dim; ++i )
           {
           second_state.velocities[i] =
           first_state.velocities[i] +
           (  first_state.accelerations[i] + second_state.accelerations[i] ) * 0.5 * delta_t;
           }
           }
           for (  size_t i = 0; i < dim; ++i )
           {
           // second state velocity should be reached on the end of the segment,   so use middle
           second_state.positions[i] =
           first_state.positions[i] +
           (  first_state.velocities[i] + second_state.velocities[i] ) * 0.5 * delta_t;
           }
           }
          }
          
     335  TrajectoryPointConstIter Trajectory::begin(   ) const
          {
           THROW_ON_NULLPTR(  trajectory_msg_ )
          
           return trajectory_msg_->points.begin(   );
          }
          
     342  TrajectoryPointConstIter Trajectory::end(   ) const
          {
           THROW_ON_NULLPTR(  trajectory_msg_ )
          
           return trajectory_msg_->points.end(   );
          }
          
     349  rclcpp::Time Trajectory::time_from_start(   ) const { return trajectory_start_time_; }
          
     351  bool Trajectory::has_trajectory_msg(   ) const { return trajectory_msg_.get(   ) != nullptr; }
          
          } // namespace joint_trajectory_controller

ros2_controllers/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/utilities.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      25  TEST(  TestLoadJointStateController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_joint_trajectory_controller",   "joint_trajectory_controller/JointTrajectoryController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/joint_trajectory_controller/test/test_trajectory.cpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <chrono>
          #include <cmath>
          #include <memory>
          #include <vector>
          
          #include "gtest/gtest.h"
          
          #include "builtin_interfaces/msg/duration.hpp"
          #include "builtin_interfaces/msg/time.hpp"
          #include "joint_trajectory_controller/trajectory.hpp"
          #include "rclcpp/clock.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/time.hpp"
          #include "std_msgs/msg/header.hpp"
          #include "trajectory_msgs/msg/joint_trajectory.hpp"
          #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
          
          using namespace joint_trajectory_controller::interpolation_methods; // NOLINT
          using namespace std::chrono_literals;
          
          namespace
          {
          // Floating-point value comparison threshold
          const double EPS = 1e-8;
          } // namespace
          
      41  TEST(  TestTrajectory,   initialize_trajectory )
          {
           {
           auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           empty_msg->header.stamp.sec = 2;
           empty_msg->header.stamp.nanosec = 2;
           const rclcpp::Time empty_time = empty_msg->header.stamp;
           auto traj = joint_trajectory_controller::Trajectory(  empty_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_point;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
           traj.sample(  rclcpp::Clock(   ).now(   ),   DEFAULT_INTERPOLATION,   expected_point,   start,   end );
          
           EXPECT_EQ(  traj.end(   ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           EXPECT_EQ(  empty_time,   traj.time_from_start(   ) );
           }
           {
           auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           empty_msg->header.stamp.sec = 0;
           empty_msg->header.stamp.nanosec = 0;
           const auto now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  empty_msg );
           const auto traj_starttime = traj.time_from_start(   );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_point;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
           traj.sample(  rclcpp::Clock(   ).now(   ),   DEFAULT_INTERPOLATION,   expected_point,   start,   end );
          
           EXPECT_EQ(  traj.end(   ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           const auto allowed_delta = 10000ll;
           EXPECT_LT(  traj.time_from_start(   ).nanoseconds(   ) - now.nanoseconds(   ),   allowed_delta );
           }
          }
          
      77  TEST(  TestTrajectory,   sample_trajectory_positions )
          {
           auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           full_msg->header.stamp = rclcpp::Time(  0 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p1;
           p1.positions.push_back(  1.0 );
           p1.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           full_msg->points.push_back(  p1 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p2;
           p2.positions.push_back(  2.0 );
           p2.time_from_start = rclcpp::Duration::from_seconds(  2.0 );
           full_msg->points.push_back(  p2 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p3;
           p3.positions.push_back(  3.0 );
           p3.time_from_start = rclcpp::Duration::from_seconds(  3.0 );
           full_msg->points.push_back(  p3 );
          
           trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
           point_before_msg.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point_before_msg.positions.push_back(  0.0 );
          
           // set current state before trajectory msg was sent
           const rclcpp::Time time_now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  time_now,   point_before_msg,   full_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
          
           double duration_first_seg = 1.0;
           double velocity = (  p1.positions[0] - point_before_msg.positions[0] ) / duration_first_seg;
          
           // sample at trajectory starting time
           {
           traj.sample(  time_now,   DEFAULT_INTERPOLATION,   expected_state,   start,   end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  traj.begin(   ),   end );
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample before time_now
           {
           bool result = traj.sample(  
           time_now - rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           ASSERT_EQ(  result,   false );
           }
          
           // sample 0.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  traj.begin(   ),   end );
           double half_current_to_p1 = (  point_before_msg.positions[0] + p1.positions[0] ) * 0.5;
           EXPECT_NEAR(  half_current_to_p1,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample 1s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  p1.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample 1.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  (  ++traj.begin(   ) ),   end );
           double half_p1_to_p2 = (  p1.positions[0] + p2.positions[0] ) * 0.5;
           EXPECT_NEAR(  half_p1_to_p2,   expected_state.positions[0],   EPS );
           }
          
           // sample 2.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  2.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           double half_p2_to_p3 = (  p2.positions[0] + p3.positions[0] ) * 0.5;
           EXPECT_NEAR(  half_p2_to_p3,   expected_state.positions[0],   EPS );
           }
          
           // sample 3s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_NEAR(  p3.positions[0],   expected_state.positions[0],   EPS );
           }
          
           // sample past given points
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.125 ),   DEFAULT_INTERPOLATION,   expected_state,  
           start,   end );
           ASSERT_EQ(  (  --traj.end(   ) ),   start );
           ASSERT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  p3.positions[0],   expected_state.positions[0],   EPS );
           }
          }
          
     193  TEST(  TestTrajectory,   interpolation_pos_vel )
          {
           // taken from ros1_controllers QuinticSplineSegmentTest::PosVelEnpointsSampler
          
           // Start and end state taken from x^3 - 2x
           trajectory_msgs::msg::JointTrajectoryPoint start_state;
           start_state.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           start_state.positions.push_back(  0.0 );
           start_state.velocities.push_back(  -2.0 );
           start_state.accelerations.clear(   );
          
           trajectory_msgs::msg::JointTrajectoryPoint end_state;
           end_state.time_from_start = rclcpp::Duration::from_seconds(  3.0 );
           end_state.positions.push_back(  4.0 );
           end_state.velocities.push_back(  10.0 );
           end_state.accelerations.push_back(  0.0 ); // Should be ignored,   start state does not specify it
          
           auto traj = joint_trajectory_controller::Trajectory(   );
           rclcpp::Time time_now(  0 );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
          
           // sample at start_time
           {
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + start_state.time_from_start,   expected_state );
           EXPECT_NEAR(  start_state.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  start_state.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          
           // Sample at mid-segment: Zero-crossing
           {
           auto t = rclcpp::Duration::from_seconds(  std::sqrt(  2.0 ) );
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + start_state.time_from_start + t,   expected_state );
           EXPECT_NEAR(  0.0,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  4.0,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  6.0 * std::sqrt(  2.0 ),   expected_state.accelerations[0],   EPS );
           }
          
           // sample at end_time
           {
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + end_state.time_from_start,   expected_state );
           EXPECT_NEAR(  end_state.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  end_state.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  12.0,   expected_state.accelerations[0],   EPS );
           }
          }
          
     247  TEST(  TestTrajectory,   interpolation_pos_vel_accel )
          {
           // taken from ros1_controllers QuinticSplineSegmentTest::PosVeAcclEnpointsSampler
          
           // Start and end state taken from x(  x-1 )(  x-2 )(  x-3 )(  x-4 ) = x^5 -10x^4 + 35x^3 -50x^2 + 24x
           trajectory_msgs::msg::JointTrajectoryPoint start_state;
           start_state.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           start_state.positions.push_back(  0.0 );
           start_state.velocities.push_back(  24.0 );
           start_state.accelerations.push_back(  -100.0 );
          
           trajectory_msgs::msg::JointTrajectoryPoint end_state;
           end_state.time_from_start = rclcpp::Duration::from_seconds(  3.0 );
           end_state.positions.push_back(  0.0 );
           end_state.velocities.push_back(  4.0 );
           end_state.accelerations.push_back(  0.0 );
          
           auto traj = joint_trajectory_controller::Trajectory(   );
           rclcpp::Time time_now(  0 );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
          
           // sample at start_time
           {
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + start_state.time_from_start,   expected_state );
           EXPECT_NEAR(  start_state.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  start_state.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  start_state.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          
           // Sample at mid-segment: Zero-crossing
           {
           auto t = rclcpp::Duration::from_seconds(  1.0 );
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + start_state.time_from_start + t,   expected_state );
           EXPECT_NEAR(  0.0,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  -6.0,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  10.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample at end_time
           {
           traj.interpolate_between_points(  
           time_now + start_state.time_from_start,   start_state,   time_now + end_state.time_from_start,  
           end_state,   time_now + end_state.time_from_start,   expected_state );
           EXPECT_NEAR(  end_state.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  end_state.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  end_state.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          }
          
     301  TEST(  TestTrajectory,   sample_trajectory_velocity_with_interpolation )
          {
           auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           full_msg->header.stamp = rclcpp::Time(  0 );
          
           // definitions
           double time_first_seg = 1.0;
           double time_second_seg = 2.0;
           double time_third_seg = 3.0;
           double velocity_first_seg = 1.0;
           double velocity_second_seg = 2.0;
           double velocity_third_seg = 1.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint p1;
           p1.velocities.push_back(  velocity_first_seg );
           p1.time_from_start = rclcpp::Duration::from_seconds(  time_first_seg );
           full_msg->points.push_back(  p1 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p2;
           p2.velocities.push_back(  velocity_second_seg );
           p2.time_from_start = rclcpp::Duration::from_seconds(  time_second_seg );
           full_msg->points.push_back(  p2 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p3;
           p3.velocities.push_back(  velocity_third_seg );
           p3.time_from_start = rclcpp::Duration::from_seconds(  time_third_seg );
           full_msg->points.push_back(  p3 );
          
           trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
           point_before_msg.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point_before_msg.positions.push_back(  0.0 );
           point_before_msg.velocities.push_back(  0.0 );
          
           // set current state before trajectory msg was sent
           const rclcpp::Time time_now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  time_now,   point_before_msg,   full_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
          
           // sample at trajectory starting time
           {
           traj.sample(  time_now,   DEFAULT_INTERPOLATION,   expected_state,   start,   end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  traj.begin(   ),   end );
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  point_before_msg.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  (  velocity_first_seg / time_first_seg ),   expected_state.accelerations[0],   EPS );
           }
          
           // sample before time_now
           {
           bool result = traj.sample(  
           time_now - rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  result,   false );
           }
          
           // sample 0.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  traj.begin(   ),   end );
           double half_current_to_p1 =
           point_before_msg.positions[0] +
           (  point_before_msg.velocities[0] +
           (  (  point_before_msg.velocities[0] + p1.velocities[0] ) / 2 - point_before_msg.velocities[0] ) /
           2 ) *
           0.5;
           EXPECT_NEAR(  half_current_to_p1,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p1.velocities[0] / 2,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  (  velocity_first_seg / time_first_seg ),   expected_state.accelerations[0],   EPS );
           }
          
           // sample 1s after msg
           double position_first_seg =
           point_before_msg.positions[0] + (  0.0 + p1.velocities[0] ) / 2 * time_first_seg;
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  position_first_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p1.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  
           (  velocity_second_seg - velocity_first_seg / (  time_second_seg - time_first_seg ) ),  
           expected_state.accelerations[0],   EPS );
           }
          
           // sample 1.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   end );
           double half_p1_to_p2 =
           position_first_seg +
           (  p1.velocities[0] + (  (  p1.velocities[0] + p2.velocities[0] ) / 2 - p1.velocities[0] ) / 2 ) * 0.5;
           EXPECT_NEAR(  half_p1_to_p2,   expected_state.positions[0],   EPS );
           double half_p1_to_p2_vel = (  p1.velocities[0] + p2.velocities[0] ) / 2;
           EXPECT_NEAR(  half_p1_to_p2_vel,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  
           (  velocity_second_seg - velocity_first_seg / (  time_second_seg - time_first_seg ) ),  
           expected_state.accelerations[0],   EPS );
           }
          
           // sample 2s after msg
           double position_second_seg = position_first_seg + (  p1.velocities[0] + p2.velocities[0] ) / 2 *
           (  time_second_seg - time_first_seg );
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  2 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   start );
           EXPECT_EQ(  (  --traj.end(   ) ),   end );
           EXPECT_NEAR(  position_second_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p2.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  
           (  velocity_third_seg - velocity_second_seg / (  time_third_seg - time_second_seg ) ),  
           expected_state.accelerations[0],   EPS );
           }
          
           // sample 2.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  2.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   start );
           EXPECT_EQ(  (  --traj.end(   ) ),   end );
           double half_p2_to_p3 =
           position_second_seg +
           (  p2.velocities[0] + (  (  p2.velocities[0] + p3.velocities[0] ) / 2 - p2.velocities[0] ) / 2 ) * 0.5;
           EXPECT_NEAR(  half_p2_to_p3,   expected_state.positions[0],   EPS );
           double half_p2_to_p3_vel = (  p2.velocities[0] + p3.velocities[0] ) / 2;
           EXPECT_NEAR(  half_p2_to_p3_vel,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  
           (  velocity_third_seg - velocity_second_seg / (  time_third_seg - time_second_seg ) ),  
           expected_state.accelerations[0],   EPS );
           }
          
           // sample 3s after msg
           double position_third_seg = position_second_seg + (  p2.velocities[0] + p3.velocities[0] ) / 2 *
           (  time_third_seg - time_second_seg );
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  (  --traj.end(   ) ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  position_third_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p3.velocities[0],   expected_state.velocities[0],   EPS );
           // the goal is reached so no acceleration anymore
           EXPECT_NEAR(  0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample past given points - movement virtually stops
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.125 ),   DEFAULT_INTERPOLATION,   expected_state,  
           start,   end );
           EXPECT_EQ(  (  --traj.end(   ) ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  position_third_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p3.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          }
          
          // This test is added because previous one behaved strange if
          // "point_before_msg.velocities.push_back(  0.0 );" was not defined
     475  TEST(  TestTrajectory,   sample_trajectory_velocity_with_interpolation_strange_without_vel )
          {
           auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           full_msg->header.stamp = rclcpp::Time(  0 );
          
           // definitions
           double time_first_seg = 1.0;
           double time_second_seg = 2.0;
           double time_third_seg = 3.0;
           double velocity_first_seg = 1.0;
           double velocity_second_seg = 2.0;
           double velocity_third_seg = 1.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint p1;
           p1.velocities.push_back(  velocity_first_seg );
           p1.time_from_start = rclcpp::Duration::from_seconds(  time_first_seg );
           full_msg->points.push_back(  p1 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p2;
           p2.velocities.push_back(  velocity_second_seg );
           p2.time_from_start = rclcpp::Duration::from_seconds(  time_second_seg );
           full_msg->points.push_back(  p2 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p3;
           p3.velocities.push_back(  velocity_third_seg );
           p3.time_from_start = rclcpp::Duration::from_seconds(  time_third_seg );
           full_msg->points.push_back(  p3 );
          
           trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
           point_before_msg.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point_before_msg.positions.push_back(  0.0 );
          
           // set current state before trajectory msg was sent
           const rclcpp::Time time_now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  time_now,   point_before_msg,   full_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
          
           // sample at trajectory starting time
           {
           traj.sample(  time_now,   DEFAULT_INTERPOLATION,   expected_state,   start,   end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  traj.begin(   ),   end );
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.velocities[0],   EPS );
           // is 0 because point_before_msg does not have velocity defined
           EXPECT_NEAR(  1.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample before time_now
           {
           bool result = traj.sample(  
           time_now - rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  result,   false );
           }
          
           // sample 0.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  traj.begin(   ),   end );
           // double half_current_to_p1 = point_before_msg.positions[0] +
           // (  point_before_msg.velocities[0] +
           // (  (  point_before_msg.velocities[0] + p1.velocities[0] ) / 2 -
           // point_before_msg.velocities[0] ) / 2 ) * 0.5;
           double half_current_to_p1 =
           point_before_msg.positions[0] + (  0.0 + (  (  0.0 + p1.velocities[0] ) / 2 - 0.0 ) / 2 ) * 0.5;
           EXPECT_NEAR(  half_current_to_p1,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p1.velocities[0] / 2,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  1.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample 1s after msg
           double position_first_seg =
           point_before_msg.positions[0] + (  0.0 + p1.velocities[0] ) / 2 * time_first_seg;
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  position_first_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  p1.velocities[0],   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  1.0,   expected_state.accelerations[0],   EPS );
           }
          }
          
     566  TEST(  TestTrajectory,   sample_trajectory_acceleration_with_interpolation )
          {
           auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           full_msg->header.stamp = rclcpp::Time(  0 );
          
           // definitions
           double time_first_seg = 1.0;
           double time_second_seg = 2.0;
           double time_third_seg = 3.0;
           double acceleration_first_seg = 1.0;
           double acceleration_second_seg = 2.0;
           double acceleration_third_seg = 1.0;
          
           trajectory_msgs::msg::JointTrajectoryPoint p1;
           p1.accelerations.push_back(  acceleration_first_seg );
           p1.time_from_start = rclcpp::Duration::from_seconds(  time_first_seg );
           full_msg->points.push_back(  p1 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p2;
           p2.accelerations.push_back(  acceleration_second_seg );
           p2.time_from_start = rclcpp::Duration::from_seconds(  time_second_seg );
           full_msg->points.push_back(  p2 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p3;
           p3.accelerations.push_back(  acceleration_third_seg );
           p3.time_from_start = rclcpp::Duration::from_seconds(  time_third_seg );
           full_msg->points.push_back(  p3 );
          
           trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
           point_before_msg.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point_before_msg.positions.push_back(  0.0 );
           point_before_msg.velocities.push_back(  0.0 );
          
           // set current state before trajectory msg was sent
           const rclcpp::Time time_now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  time_now,   point_before_msg,   full_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
          
           // sample at trajectory starting time
           {
           traj.sample(  time_now,   DEFAULT_INTERPOLATION,   expected_state,   start,   end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  traj.begin(   ),   end );
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  0.0,   expected_state.velocities[0],   EPS );
           // is 0 because point_before_msg does not have velocity defined
           EXPECT_NEAR(  0.0,   expected_state.accelerations[0],   EPS );
           }
          
           // sample before time_now
           {
           bool result = traj.sample(  
           time_now - rclcpp::Duration::from_seconds(  0.5 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  result,   false );
           }
          
           // Sample only on points testing of intermediate values is too complex and not necessary
          
           // sample 1s after msg
           double velocity_first_seg =
           point_before_msg.velocities[0] + (  0.0 + p1.accelerations[0] ) / 2 * time_first_seg;
           double position_first_seg =
           point_before_msg.positions[0] + (  0.0 + velocity_first_seg ) / 2 * time_first_seg;
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  traj.begin(   ),   start );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  position_first_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity_first_seg,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  p1.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          
           // sample 2s after msg
           double velocity_second_seg = velocity_first_seg + (  p1.accelerations[0] + p2.accelerations[0] ) /
           2 * (  time_second_seg - time_first_seg );
           double position_second_seg = position_first_seg + (  velocity_first_seg + velocity_second_seg ) / 2 *
           (  time_second_seg - time_first_seg );
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  2 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  (  ++traj.begin(   ) ),   start );
           EXPECT_EQ(  (  --traj.end(   ) ),   end );
           EXPECT_NEAR(  position_second_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity_second_seg,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  p2.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          
           // sample 3s after msg
           double velocity_third_seg = velocity_second_seg + (  p2.accelerations[0] + p3.accelerations[0] ) /
           2 * (  time_third_seg - time_second_seg );
           double position_third_seg = position_second_seg + (  velocity_second_seg + velocity_third_seg ) / 2 *
           (  time_third_seg - time_second_seg );
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.0 ),   DEFAULT_INTERPOLATION,   expected_state,   start,  
           end );
           EXPECT_EQ(  (  --traj.end(   ) ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  position_third_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity_third_seg,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  p3.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          
           // sample past given points - movement virtually stops
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.125 ),   DEFAULT_INTERPOLATION,   expected_state,  
           start,   end );
           EXPECT_EQ(  (  --traj.end(   ) ),   start );
           EXPECT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  position_third_seg,   expected_state.positions[0],   EPS );
           EXPECT_NEAR(  velocity_third_seg,   expected_state.velocities[0],   EPS );
           EXPECT_NEAR(  p3.accelerations[0],   expected_state.accelerations[0],   EPS );
           }
          }
          
     688  TEST(  TestTrajectory,   skip_interpolation )
          {
           // Simple passthrough without extra interpolation
           {
           const InterpolationMethod no_interpolation = InterpolationMethod::NONE;
          
           auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>(   );
           full_msg->header.stamp = rclcpp::Time(  0 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p1;
           p1.positions.push_back(  1.0 );
           p1.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           full_msg->points.push_back(  p1 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p2;
           p2.positions.push_back(  2.0 );
           p2.time_from_start = rclcpp::Duration::from_seconds(  2.0 );
           full_msg->points.push_back(  p2 );
          
           trajectory_msgs::msg::JointTrajectoryPoint p3;
           p3.positions.push_back(  3.0 );
           p3.time_from_start = rclcpp::Duration::from_seconds(  3.0 );
           full_msg->points.push_back(  p3 );
          
           trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
           point_before_msg.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point_before_msg.positions.push_back(  0.0 );
          
           // set current state before trajectory msg was sent
           const rclcpp::Time time_now = rclcpp::Clock(   ).now(   );
           auto traj = joint_trajectory_controller::Trajectory(  time_now,   point_before_msg,   full_msg );
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_state;
           joint_trajectory_controller::TrajectoryPointConstIter start,   end;
          
           // sample at trajectory starting time
           {
           traj.sample(  time_now,   no_interpolation,   expected_state,   start,   end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  traj.begin(   ),   end );
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           // There were no vels/accels in the input,   so they should remain empty
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.velocities.size(   ) );
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.accelerations.size(   ) );
           }
          
           // sample before time_now
           {
           bool result = traj.sample(  
           time_now - rclcpp::Duration::from_seconds(  0.5 ),   no_interpolation,   expected_state,   start,  
           end );
           ASSERT_EQ(  result,   false );
           }
          
           // sample 0.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  0.5 ),   no_interpolation,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  traj.begin(   ),   end );
           // For passthrough,   this should just return the first waypoint
           EXPECT_NEAR(  point_before_msg.positions[0],   expected_state.positions[0],   EPS );
           // There were no vels/accels in the input,   so they should remain empty
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.velocities.size(   ) );
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.accelerations.size(   ) );
           }
          
           // sample 1s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.0 ),   no_interpolation,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  p2.positions[0],   expected_state.positions[0],   EPS );
           // There were no vels/accels in the input,   so they should remain empty
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.velocities.size(   ) );
           EXPECT_EQ(  
           static_cast<std::make_unsigned<decltype(  0 )>::type>(  0 ),   expected_state.accelerations.size(   ) );
           }
          
           // sample 1.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  1.5 ),   no_interpolation,   expected_state,   start,  
           end );
           ASSERT_EQ(  traj.begin(   ),   start );
           ASSERT_EQ(  (  ++traj.begin(   ) ),   end );
           EXPECT_NEAR(  p2.positions[0],   expected_state.positions[0],   EPS );
           }
          
           // sample 2.5s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  2.5 ),   no_interpolation,   expected_state,   start,  
           end );
           EXPECT_NEAR(  p3.positions[0],   expected_state.positions[0],   EPS );
           }
          
           // sample 3s after msg
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.0 ),   no_interpolation,   expected_state,   start,  
           end );
           EXPECT_NEAR(  p3.positions[0],   expected_state.positions[0],   EPS );
           }
          
           // sample past given points
           {
           traj.sample(  
           time_now + rclcpp::Duration::from_seconds(  3.125 ),   no_interpolation,   expected_state,   start,  
           end );
           ASSERT_EQ(  (  --traj.end(   ) ),   start );
           ASSERT_EQ(  traj.end(   ),   end );
           EXPECT_NEAR(  p3.positions[0],   expected_state.positions[0],   EPS );
           }
           }
          }

ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef _MSC_VER
          #include <cxxabi.h>
          #endif
          #include <algorithm>
          #include <chrono>
          #include <functional>
          #include <future>
          #include <memory>
          #include <ratio>
          #include <stdexcept>
          #include <string>
          #include <system_error>
          #include <thread>
          #include <vector>
          
          #include "action_msgs/msg/goal_status_array.hpp"
          #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp"
          #include "controller_interface/controller_interface.hpp"
          #include "gtest/gtest.h"
          #include "hardware_interface/resource_manager.hpp"
          #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
          #include "rclcpp/clock.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/executors/multi_threaded_executor.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/node.hpp"
          #include "rclcpp/parameter.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_action/client.hpp"
          #include "rclcpp_action/client_goal_handle.hpp"
          #include "rclcpp_action/create_client.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "test_trajectory_controller_utils.hpp"
          #include "trajectory_msgs/msg/joint_trajectory.hpp"
          #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
          
          using std::placeholders::_1;
          using std::placeholders::_2;
          using test_trajectory_controllers::TestableJointTrajectoryController;
          using test_trajectory_controllers::TrajectoryControllerTest;
          using trajectory_msgs::msg::JointTrajectoryPoint;
          
      58  class TestTrajectoryActions : public TrajectoryControllerTest
          {
          protected:
      61   void SetUp(   )
           {
           TrajectoryControllerTest::SetUp(   );
           goal_options_.result_callback =
           std::bind(  &TestTrajectoryActions::common_result_response,   this,   _1 );
           goal_options_.feedback_callback = nullptr;
           }
          
      69   void SetUpExecutor(  const std::vector<rclcpp::Parameter> & parameters = {} )
           {
           setup_executor_ = true;
          
           executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>(   );
          
           SetUpAndActivateTrajectoryController(  true,   parameters );
          
           executor_->add_node(  traj_controller_->get_node(   )->get_node_base_interface(   ) );
          
           SetUpActionClient(   );
          
           executor_->add_node(  node_->get_node_base_interface(   ) );
          
           executor_future_handle_ = std::async(  std::launch::async,   [&](   ) -> void { executor_->spin(   ); } );
           }
          
      86   void SetUpControllerHardware(   )
           {
           setup_controller_hw_ = true;
          
           controller_hw_thread_ = std::thread(  [&](   ) {
           // controller hardware cycle update loop
           auto start_time = rclcpp::Clock(   ).now(   );
           rclcpp::Duration wait = rclcpp::Duration::from_seconds(  2.0 );
           auto end_time = start_time + wait;
           while (  rclcpp::Clock(   ).now(   ) < end_time )
           {
           traj_controller_->update(  rclcpp::Clock(   ).now(   ),   rclcpp::Clock(   ).now(   ) - start_time );
           }
           } );
          
           // sometimes doesn't receive calls when we don't sleep
           std::this_thread::sleep_for(  std::chrono::milliseconds(  300 ) );
           }
          
     105   void SetUpActionClient(   )
           {
           action_client_ = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(  
           node_->get_node_base_interface(   ),   node_->get_node_graph_interface(   ),  
           node_->get_node_logging_interface(   ),   node_->get_node_waitables_interface(   ),  
           controller_name_ + "/follow_joint_trajectory" );
          
           bool response = action_client_->wait_for_action_server(  std::chrono::seconds(  1 ) );
           if (  !response )
           {
           throw std::runtime_error(  "could not get action server" );
           }
           }
          
     119   static void TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
     121   void TearDown(   )
           {
           TearDownControllerHardware(   );
           TearDownExecutor(   );
           }
          
     127   void TearDownExecutor(   )
           {
           if (  setup_executor_ )
           {
           setup_executor_ = false;
           executor_->cancel(   );
           executor_future_handle_.wait(   );
           }
           }
          
     137   void TearDownControllerHardware(   )
           {
           if (  setup_controller_hw_ )
           {
           setup_controller_hw_ = false;
           if (  controller_hw_thread_.joinable(   ) )
           {
           controller_hw_thread_.join(   );
           }
           }
           }
          
           using FollowJointTrajectoryMsg = control_msgs::action::FollowJointTrajectory;
           using GoalHandle = rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>;
           using GoalOptions = rclcpp_action::Client<FollowJointTrajectoryMsg>::SendGoalOptions;
          
           std::shared_future<typename GoalHandle::SharedPtr> sendActionGoal(  
           const std::vector<JointTrajectoryPoint> & points,   double timeout,   const GoalOptions & opt )
           {
           control_msgs::action::FollowJointTrajectory_Goal goal_msg;
           goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(  timeout );
           goal_msg.trajectory.joint_names = joint_names_;
           goal_msg.trajectory.points = points;
          
           return action_client_->async_send_goal(  goal_msg,   opt );
           }
          
           rclcpp_action::Client<FollowJointTrajectoryMsg>::SharedPtr action_client_;
           rclcpp_action::ResultCode common_resultcode_ = rclcpp_action::ResultCode::UNKNOWN;
           int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
          
           bool setup_executor_ = false;
           rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_;
           std::future<void> executor_future_handle_;
          
           bool setup_controller_hw_ = false;
           std::thread controller_hw_thread_;
          
           GoalOptions goal_options_;
          
          public:
           void common_result_response(  const GoalHandle::WrappedResult & result )
           {
           RCLCPP_DEBUG(  
           node_->get_logger(   ),   "common_result_response time: %f",   rclcpp::Clock(   ).now(   ).seconds(   ) );
           common_resultcode_ = result.code;
           common_action_result_code_ = result.result->error_code;
           switch (  result.code )
           {
           case rclcpp_action::ResultCode::SUCCEEDED:
           break;
           case rclcpp_action::ResultCode::ABORTED:
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Goal was aborted" );
           return;
           case rclcpp_action::ResultCode::CANCELED:
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Goal was canceled" );
           return;
           default:
           RCLCPP_DEBUG(  node_->get_logger(   ),   "Unknown result code" );
           return;
           }
           }
          };
          
          TEST_F(  TestTrajectoryActions,   test_success_single_point_sendgoal )
          {
           SetUpExecutor(   );
           SetUpControllerHardware(   );
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  0.5 );
           point.positions.resize(  joint_names_.size(   ) );
          
           point.positions[0] = 1.0;
           point.positions[1] = 2.0;
           point.positions[2] = 3.0;
          
           points.push_back(  point );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::SUCCEEDED,   common_resultcode_ );
          
           EXPECT_EQ(  1.0,   joint_pos_[0] );
           EXPECT_EQ(  2.0,   joint_pos_[1] );
           EXPECT_EQ(  3.0,   joint_pos_[2] );
          }
          
          TEST_F(  TestTrajectoryActions,   test_success_multi_point_sendgoal )
          {
           SetUpExecutor(   );
           SetUpControllerHardware(   );
          
           // add feedback
           bool feedback_recv = false;
           goal_options_.feedback_callback =
           [&](  
           rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,  
           const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> ) { feedback_recv = true; };
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal with multiple points
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point1;
           point1.time_from_start = rclcpp::Duration::from_seconds(  0.2 );
           point1.positions.resize(  joint_names_.size(   ) );
          
           point1.positions[0] = 4.0;
           point1.positions[1] = 5.0;
           point1.positions[2] = 6.0;
           points.push_back(  point1 );
          
           JointTrajectoryPoint point2;
           point2.time_from_start = rclcpp::Duration::from_seconds(  0.3 );
           point2.positions.resize(  joint_names_.size(   ) );
          
           point2.positions[0] = 7.0;
           point2.positions[1] = 8.0;
           point2.positions[2] = 9.0;
           points.push_back(  point2 );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  feedback_recv );
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::SUCCEEDED,   common_resultcode_ );
          
           EXPECT_NEAR(  7.0,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  8.0,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  9.0,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_goal_tolerances_single_point_success )
          {
           // set tolerance parameters
           std::vector<rclcpp::Parameter> params = {
           rclcpp::Parameter(  "constraints.joint1.goal",   0.1 ),  
           rclcpp::Parameter(  "constraints.joint2.goal",   0.1 ),  
           rclcpp::Parameter(  "constraints.joint3.goal",   0.1 )};
          
           SetUpExecutor(  params );
           SetUpControllerHardware(   );
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  0.5 );
           point.positions.resize(  joint_names_.size(   ) );
          
           point.positions[0] = 1.0;
           point.positions[1] = 2.0;
           point.positions[2] = 3.0;
           points.push_back(  point );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::SUCCEEDED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL,   common_action_result_code_ );
          
           EXPECT_NEAR(  1.0,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  2.0,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  3.0,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_goal_tolerances_multi_point_success )
          {
           // set tolerance parameters
           std::vector<rclcpp::Parameter> params = {
           rclcpp::Parameter(  "constraints.joint1.goal",   0.1 ),  
           rclcpp::Parameter(  "constraints.joint2.goal",   0.1 ),  
           rclcpp::Parameter(  "constraints.joint3.goal",   0.1 )};
          
           SetUpExecutor(  params );
           SetUpControllerHardware(   );
          
           // add feedback
           bool feedback_recv = false;
           goal_options_.feedback_callback =
           [&](  
           rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,  
           const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> ) { feedback_recv = true; };
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal with multiple points
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point1;
           point1.time_from_start = rclcpp::Duration::from_seconds(  0.2 );
           point1.positions.resize(  joint_names_.size(   ) );
          
           point1.positions[0] = 4.0;
           point1.positions[1] = 5.0;
           point1.positions[2] = 6.0;
           points.push_back(  point1 );
          
           JointTrajectoryPoint point2;
           point2.time_from_start = rclcpp::Duration::from_seconds(  0.3 );
           point2.positions.resize(  joint_names_.size(   ) );
          
           point2.positions[0] = 7.0;
           point2.positions[1] = 8.0;
           point2.positions[2] = 9.0;
           points.push_back(  point2 );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  feedback_recv );
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::SUCCEEDED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL,   common_action_result_code_ );
          
           EXPECT_NEAR(  7.0,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  8.0,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  9.0,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_state_tolerances_fail )
          {
           // set joint tolerance parameters
           const double state_tol = 0.0001;
           std::vector<rclcpp::Parameter> params = {
           rclcpp::Parameter(  "constraints.joint1.trajectory",   state_tol ),  
           rclcpp::Parameter(  "constraints.joint2.trajectory",   state_tol ),  
           rclcpp::Parameter(  "constraints.joint3.trajectory",   state_tol )};
          
           SetUpExecutor(  params );
           SetUpControllerHardware(   );
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point1;
           point1.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point1.positions.resize(  joint_names_.size(   ) );
          
           point1.positions[0] = 4.0;
           point1.positions[1] = 5.0;
           point1.positions[2] = 6.0;
           points.push_back(  point1 );
          
           JointTrajectoryPoint point2;
           point2.time_from_start = rclcpp::Duration::from_seconds(  0.1 );
           point2.positions.resize(  joint_names_.size(   ) );
          
           point2.positions[0] = 7.0;
           point2.positions[1] = 8.0;
           point2.positions[2] = 9.0;
           points.push_back(  point2 );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::ABORTED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED,  
           common_action_result_code_ );
          
           // run an update,   it should be holding
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           EXPECT_NEAR(  INITIAL_POS_JOINT1,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  INITIAL_POS_JOINT2,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  INITIAL_POS_JOINT3,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_goal_tolerances_fail )
          {
           // set joint tolerance parameters
           const double goal_tol = 0.1;
           // set very small goal_time so that goal_time is violated
           const double goal_time = 0.000001;
           std::vector<rclcpp::Parameter> params = {
           rclcpp::Parameter(  "constraints.joint1.goal",   goal_tol ),  
           rclcpp::Parameter(  "constraints.joint2.goal",   goal_tol ),  
           rclcpp::Parameter(  "constraints.joint3.goal",   goal_tol ),  
           rclcpp::Parameter(  "constraints.goal_time",   goal_time )};
          
           SetUpExecutor(  params );
           SetUpControllerHardware(   );
          
           const double init_pos1 = joint_pos_[0];
           const double init_pos2 = joint_pos_[1];
           const double init_pos3 = joint_pos_[2];
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point.positions.resize(  joint_names_.size(   ) );
          
           point.positions[0] = 4.0;
           point.positions[1] = 5.0;
           point.positions[2] = 6.0;
           points.push_back(  point );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::ABORTED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED,  
           common_action_result_code_ );
          
           // run an update,   it should be holding
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           EXPECT_NEAR(  init_pos1,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  init_pos2,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  init_pos3,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_no_time_from_start_state_tolerance_fail )
          {
           // set joint tolerance parameters
           const double state_tol = 0.0001;
           std::vector<rclcpp::Parameter> params = {
           rclcpp::Parameter(  "constraints.joint1.trajectory",   state_tol ),  
           rclcpp::Parameter(  "constraints.joint2.trajectory",   state_tol ),  
           rclcpp::Parameter(  "constraints.joint3.trajectory",   state_tol )};
          
           SetUpExecutor(  params );
           SetUpControllerHardware(   );
          
           const double init_pos1 = joint_pos_[0];
           const double init_pos2 = joint_pos_[1];
           const double init_pos3 = joint_pos_[2];
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  0.0 );
           point.positions.resize(  joint_names_.size(   ) );
          
           point.positions[0] = 4.0;
           point.positions[1] = 5.0;
           point.positions[2] = 6.0;
           points.push_back(  point );
          
           gh_future = sendActionGoal(  points,   1.0,   goal_options_ );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::ABORTED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED,  
           common_action_result_code_ );
          
           // run an update,   it should be holding
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           EXPECT_NEAR(  init_pos1,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  init_pos2,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  init_pos3,   joint_pos_[2],   COMMON_THRESHOLD );
          }
          
          TEST_F(  TestTrajectoryActions,   test_cancel_hold_position )
          {
           SetUpExecutor(   );
           SetUpControllerHardware(   );
          
           std::shared_future<typename GoalHandle::SharedPtr> gh_future;
           // send goal
           {
           std::vector<JointTrajectoryPoint> points;
           JointTrajectoryPoint point;
           point.time_from_start = rclcpp::Duration::from_seconds(  1.0 );
           point.positions.resize(  joint_names_.size(   ) );
          
           point.positions[0] = 4.0;
           point.positions[1] = 5.0;
           point.positions[2] = 6.0;
           points.push_back(  point );
          
           control_msgs::action::FollowJointTrajectory_Goal goal_msg;
           goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(  2.0 );
           goal_msg.trajectory.joint_names = joint_names_;
           goal_msg.trajectory.points = points;
          
           // send and wait for half a second before cancel
           gh_future = action_client_->async_send_goal(  goal_msg,   goal_options_ );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  500 ) );
          
           const auto goal_handle = gh_future.get(   );
           action_client_->async_cancel_goal(  goal_handle );
           }
           controller_hw_thread_.join(   );
          
           EXPECT_TRUE(  gh_future.get(   ) );
           EXPECT_EQ(  rclcpp_action::ResultCode::CANCELED,   common_resultcode_ );
           EXPECT_EQ(  
           control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL,   common_action_result_code_ );
          
           const double prev_pos1 = joint_pos_[0];
           const double prev_pos2 = joint_pos_[1];
           const double prev_pos3 = joint_pos_[2];
          
           // run an update,   it should be holding
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           EXPECT_EQ(  prev_pos1,   joint_pos_[0] );
           EXPECT_EQ(  prev_pos2,   joint_pos_[1] );
           EXPECT_EQ(  prev_pos3,   joint_pos_[2] );
          }

ros2_controllers/joint_trajectory_controller/test/test_trajectory_controller.cpp

       1  // Copyright 2017 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stddef.h>
          
          #include <array>
          #include <chrono>
          #include <future>
          #include <limits>
          #include <memory>
          #include <stdexcept>
          #include <string>
          #include <system_error>
          #include <thread>
          #include <vector>
          
          #include "gtest/gtest.h"
          
          #include "builtin_interfaces/msg/duration.hpp"
          #include "builtin_interfaces/msg/time.hpp"
          #include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp"
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/clock.hpp"
          #include "rclcpp/duration.hpp"
          #include "rclcpp/executors/multi_threaded_executor.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "rclcpp/node.hpp"
          #include "rclcpp/parameter.hpp"
          #include "rclcpp/publisher.hpp"
          #include "rclcpp/qos.hpp"
          #include "rclcpp/qos_event.hpp"
          #include "rclcpp/subscription.hpp"
          #include "rclcpp/time.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/lifecycle_node.hpp"
          #include "rclcpp_lifecycle/state.hpp"
          #include "std_msgs/msg/header.hpp"
          #include "test_trajectory_controller_utils.hpp"
          #include "trajectory_msgs/msg/joint_trajectory.hpp"
          #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
          
          using lifecycle_msgs::msg::State;
          using test_trajectory_controllers::TestableJointTrajectoryController;
          using test_trajectory_controllers::TrajectoryControllerTest;
          using test_trajectory_controllers::TrajectoryControllerTestParameterized;
          
      61  bool is_same_sign(  double val1,   double val2 ) { return val1 * val2 >= 0.0; }
          
      63  void spin(  rclcpp::executors::MultiThreadedExecutor * exe ) { exe->spin(   ); }
          
      65  TEST_P(  TrajectoryControllerTestParameterized,   configure )
          {
           SetUpTrajectoryController(   );
          
           rclcpp::executors::MultiThreadedExecutor executor;
           executor.add_node(  traj_controller_->get_node(   )->get_node_base_interface(   ) );
           const auto future_handle_ = std::async(  std::launch::async,   spin,   &executor );
          
           const auto state = traj_controller_->get_node(   )->configure(   );
           ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
          
           // send msg
           builtin_interfaces::msg::Duration time_from_start;
           time_from_start.sec = 1;
           time_from_start.nanosec = 0;
           std::vector<std::vector<double>> points{{{3.3,   4.4,   5.5}}};
           publish(  time_from_start,   points );
           std::this_thread::sleep_for(  std::chrono::milliseconds(  10 ) );
          
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           // hw position == 0 because controller is not activated
           EXPECT_EQ(  0.0,   joint_pos_[0] );
           EXPECT_EQ(  0.0,   joint_pos_[1] );
           EXPECT_EQ(  0.0,   joint_pos_[2] );
          
           executor.cancel(   );
          }
          
      94  TEST_P(  TrajectoryControllerTestParameterized,   activate )
          {
           SetUpTrajectoryController(   );
          
           rclcpp::executors::MultiThreadedExecutor executor;
           executor.add_node(  traj_controller_->get_node(   )->get_node_base_interface(   ) );
          
           traj_controller_->get_node(   )->configure(   );
           ASSERT_EQ(  traj_controller_->get_state(   ).id(   ),   State::PRIMARY_STATE_INACTIVE );
          
           auto cmd_interface_config = traj_controller_->command_interface_configuration(   );
           ASSERT_EQ(  
           cmd_interface_config.names.size(   ),   joint_names_.size(   ) * command_interface_types_.size(   ) );
          
           auto state_interface_config = traj_controller_->state_interface_configuration(   );
           ASSERT_EQ(  
           state_interface_config.names.size(   ),   joint_names_.size(   ) * state_interface_types_.size(   ) );
          
           ActivateTrajectoryController(   );
           ASSERT_EQ(  traj_controller_->get_state(   ).id(   ),   State::PRIMARY_STATE_ACTIVE );
          
           executor.cancel(   );
          }
          
          // TEST_F(  TestTrajectoryController,   activation ) {
          // auto traj_controller = std::make_shared<ros_controllers::JointTrajectoryController>(  
          // joint_names_,   op_mode_ );
          // auto ret = traj_controller->init(  test_robot_,   controller_name_ );
          // if (  ret != controller_interface::return_type::OK ) {
          // FAIL(   );
          // }
          //
          // auto traj_node = traj_controller->get_node(   );
          // rclcpp::executors::MultiThreadedExecutor executor;
          // executor.add_node(  traj_node->get_node_base_interface(   ) );
          //
          // auto state = traj_controller_->configure(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
          //
          // state = traj_node->activate(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_ACTIVE );
          //
          // // wait for the subscriber and publisher to completely setup
          // std::this_thread::sleep_for(  std::chrono::seconds(  2 ) );
          //
          // // send msg
          // builtin_interfaces::msg::Duration time_from_start;
          // time_from_start.sec = 1;
          // time_from_start.nanosec = 0;
          // std::vector<std::vector<double>> points {{{3.3,   4.4,   5.5}}};
          // publish(  time_from_start,   points );
          // // wait for msg is be published to the system
          // std::this_thread::sleep_for(  std::chrono::milliseconds(  1000 ) );
          // executor.spin_once(   );
          //
          // traj_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          // resource_manager_->write(   );
          //
          // // change in hw position
          // EXPECT_EQ(  3.3,   joint_pos_[0] );
          // EXPECT_EQ(  4.4,   joint_pos_[1] );
          // EXPECT_EQ(  5.5,   joint_pos_[2] );
          //
          // executor.cancel(   );
          // }
          
          // TEST_F(  TestTrajectoryController,   reactivation ) {
          // auto traj_controller = std::make_shared<ros_controllers::JointTrajectoryController>(  
          // joint_names_,   op_mode_ );
          // auto ret = traj_controller->init(  test_robot_,   controller_name_ );
          // if (  ret != controller_interface::return_type::OK ) {
          // FAIL(   );
          // }
          //
          // auto traj_node = traj_controller->get_node(   );
          // rclcpp::executors::MultiThreadedExecutor executor;
          // executor.add_node(  traj_node->get_node_base_interface(   ) );
          //
          // auto state = traj_controller_->configure(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
          //
          // state = traj_node->activate(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_ACTIVE );
          //
          // // wait for the subscriber and publisher to completely setup
          // std::this_thread::sleep_for(  std::chrono::seconds(  2 ) );
          //
          // // send msg
          // builtin_interfaces::msg::Duration time_from_start;
          // time_from_start.sec = 1;
          // time_from_start.nanosec = 0;
          // // *INDENT-OFF*
          // std::vector<std::vector<double>> points {
          // {{3.3,   4.4,   5.5}},  
          // {{7.7,   8.8,   9.9}},  
          // {{10.10,   11.11,   12.12}}
          // };
          // // *INDENT-ON*
          // publish(  time_from_start,   points );
          // // wait for msg is be published to the system
          // std::this_thread::sleep_for(  std::chrono::milliseconds(  500 ) );
          // executor.spin_once(   );
          //
          // traj_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          // resource_manager_->write(   );
          //
          // // deactivated
          // // wait so controller process the second point when deactivated
          // std::this_thread::sleep_for(  std::chrono::milliseconds(  500 ) );
          // state = traj_controller_->deactivate(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
          // resource_manager_->read(   );
          // traj_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          // resource_manager_->write(   );
          //
          // // no change in hw position
          // EXPECT_EQ(  3.3,   joint_pos_[0] );
          // EXPECT_EQ(  4.4,   joint_pos_[1] );
          // EXPECT_EQ(  5.5,   joint_pos_[2] );
          //
          // // reactivated
          // // wait so controller process the third point when reactivated
          // std::this_thread::sleep_for(  std::chrono::milliseconds(  3000 ) );
          // state = traj_node->activate(   );
          // ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_ACTIVE );
          // resource_manager_->read(   );
          // traj_controller->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          // resource_manager_->write(   );
          //
          // // change in hw position to 3rd point
          // EXPECT_EQ(  10.10,   joint_pos_[0] );
          // EXPECT_EQ(  11.11,   joint_pos_[1] );
          // EXPECT_EQ(  12.12,   joint_pos_[2] );
          //
          // executor.cancel(   );
          // }
          
     231  TEST_P(  TrajectoryControllerTestParameterized,   cleanup )
          {
           SetUpAndActivateTrajectoryController(   );
          
           auto traj_node = traj_controller_->get_node(   );
           rclcpp::executors::MultiThreadedExecutor executor;
           executor.add_node(  traj_node->get_node_base_interface(   ) );
          
           // send msg
           builtin_interfaces::msg::Duration time_from_start;
           time_from_start.sec = 1;
           time_from_start.nanosec = 0;
           std::vector<std::vector<double>> points{{{3.3,   4.4,   5.5}}};
           publish(  time_from_start,   points );
           traj_controller_->wait_for_trajectory(  executor );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           auto state = traj_controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           state = traj_controller_->get_node(   )->cleanup(   );
           ASSERT_EQ(  State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
           // update for 0.25 seconds
           const auto start_time = rclcpp::Clock(   ).now(   );
           updateController(  rclcpp::Duration::from_seconds(  0.25 ) );
          
           // should be home pose again
           EXPECT_NEAR(  INITIAL_POS_JOINT1,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  INITIAL_POS_JOINT2,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  INITIAL_POS_JOINT3,   joint_pos_[2],   COMMON_THRESHOLD );
          
           executor.cancel(   );
          }
          
     266  TEST_P(  TrajectoryControllerTestParameterized,   correct_initialization_using_parameters )
          {
           SetUpTrajectoryController(  false );
          
           // This call is replacing the way parameters are set via launch
           SetParameters(   );
           SetParameters(   ); // This call is replacing the way parameters are set via launch
           traj_controller_->configure(   );
           auto state = traj_controller_->get_state(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
          
           ActivateTrajectoryController(   );
           rclcpp::executors::MultiThreadedExecutor executor;
           executor.add_node(  traj_controller_->get_node(   )->get_node_base_interface(   ) );
          
           state = traj_controller_->get_state(   );
           ASSERT_EQ(  State::PRIMARY_STATE_ACTIVE,   state.id(   ) );
           EXPECT_EQ(  INITIAL_POS_JOINT1,   joint_pos_[0] );
           EXPECT_EQ(  INITIAL_POS_JOINT2,   joint_pos_[1] );
           EXPECT_EQ(  INITIAL_POS_JOINT3,   joint_pos_[2] );
          
           // send msg
           constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(  250 );
           builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(  FIRST_POINT_TIME )};
           // *INDENT-OFF*
           std::vector<std::vector<double>> points{
           {{3.3,   4.4,   5.5}},   {{7.7,   8.8,   9.9}},   {{10.10,   11.11,   12.12}}};
           std::vector<std::vector<double>> points_velocities{
           {{0.01,   0.01,   0.01}},   {{0.05,   0.05,   0.05}},   {{0.06,   0.06,   0.06}}};
           // *INDENT-ON*
           publish(  time_from_start,   points,   rclcpp::Time(   ),   {},   points_velocities );
           traj_controller_->wait_for_trajectory(  executor );
          
           // first update
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           // wait so controller process the second point when deactivated
           std::this_thread::sleep_for(  FIRST_POINT_TIME );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // deactivated
           state = traj_controller_->get_node(   )->deactivate(   );
           ASSERT_EQ(  state.id(   ),   State::PRIMARY_STATE_INACTIVE );
          
           // TODO(  denis ): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong?
           // Come the flackiness here?
           const auto allowed_delta = 0.11; // 0.05;
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           EXPECT_NEAR(  3.3,   joint_pos_[0],   allowed_delta );
           EXPECT_NEAR(  4.4,   joint_pos_[1],   allowed_delta );
           EXPECT_NEAR(  5.5,   joint_pos_[2],   allowed_delta );
           }
          
           if (  traj_controller_->has_velocity_command_interface(   ) )
           {
           EXPECT_LE(  0.0,   joint_vel_[0] );
           EXPECT_LE(  0.0,   joint_vel_[1] );
           EXPECT_LE(  0.0,   joint_vel_[2] );
           }
          
           if (  traj_controller_->has_effort_command_interface(   ) )
           {
           EXPECT_LE(  0.0,   joint_eff_[0] );
           EXPECT_LE(  0.0,   joint_eff_[1] );
           EXPECT_LE(  0.0,   joint_eff_[2] );
           }
          
           // cleanup
           state = traj_controller_->get_node(   )->cleanup(   );
          
           // update loop receives a new msg and updates accordingly
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
          
           // check the traj_msg_home_ptr_ initialization code for the standard wait timing
           std::this_thread::sleep_for(  std::chrono::milliseconds(  50 ) );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           ASSERT_EQ(  State::PRIMARY_STATE_UNCONFIGURED,   state.id(   ) );
          
           EXPECT_NEAR(  INITIAL_POS_JOINT1,   joint_pos_[0],   allowed_delta );
           EXPECT_NEAR(  INITIAL_POS_JOINT2,   joint_pos_[1],   allowed_delta );
           EXPECT_NEAR(  INITIAL_POS_JOINT3,   joint_pos_[2],   allowed_delta );
          
           state = traj_controller_->get_node(   )->configure(   );
           ASSERT_EQ(  State::PRIMARY_STATE_INACTIVE,   state.id(   ) );
          
           executor.cancel(   );
          }
          
     354  TEST_P(  TrajectoryControllerTestParameterized,   state_topic_consistency )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {},   &executor );
           subscribeToState(   );
           updateController(   );
          
           // Spin to receive latest state
           executor.spin_some(   );
           auto state = getState(   );
          
           size_t n_joints = joint_names_.size(   );
          
           for (  unsigned int i = 0; i < n_joints; ++i )
           {
           EXPECT_EQ(  joint_names_[i],   state->joint_names[i] );
           }
          
           // No trajectory by default,   no desired state or error
           EXPECT_TRUE(  state->desired.positions.empty(   ) );
           EXPECT_TRUE(  state->desired.velocities.empty(   ) );
           EXPECT_TRUE(  state->desired.accelerations.empty(   ) );
          
           EXPECT_EQ(  n_joints,   state->actual.positions.size(   ) );
           if (  
           std::find(  state_interface_types_.begin(   ),   state_interface_types_.end(   ),   "velocity" ) ==
           state_interface_types_.end(   ) )
           {
           EXPECT_TRUE(  state->actual.velocities.empty(   ) );
           }
           else
           {
           EXPECT_EQ(  n_joints,   state->actual.velocities.size(   ) );
           }
           if (  
           std::find(  state_interface_types_.begin(   ),   state_interface_types_.end(   ),   "acceleration" ) ==
           state_interface_types_.end(   ) )
           {
           EXPECT_TRUE(  state->actual.accelerations.empty(   ) );
           }
           else
           {
           EXPECT_EQ(  n_joints,   state->actual.accelerations.size(   ) );
           }
          
           EXPECT_TRUE(  state->error.positions.empty(   ) );
           EXPECT_TRUE(  state->error.velocities.empty(   ) );
           EXPECT_TRUE(  state->error.accelerations.empty(   ) );
          }
          
     404  void TrajectoryControllerTest::test_state_publish_rate_target(  int target_msg_count )
          {
           rclcpp::Parameter state_publish_rate_param(  
           "state_publish_rate",   static_cast<double>(  target_msg_count ) );
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {state_publish_rate_param},   &executor );
          
           auto future_handle = std::async(  std::launch::async,   [&executor](   ) -> void { executor.spin(   ); } );
          
           using control_msgs::msg::JointTrajectoryControllerState;
          
           const int qos_level = 10;
           int echo_received_counter = 0;
           rclcpp::Subscription<JointTrajectoryControllerState>::SharedPtr subs =
           traj_controller_->get_node(   )->create_subscription<JointTrajectoryControllerState>(  
           controller_name_ + "/state",   qos_level,  
           [&](  JointTrajectoryControllerState::UniquePtr ) { ++echo_received_counter; } );
          
           // update for 1second
           const auto start_time = rclcpp::Clock(   ).now(   );
           const rclcpp::Duration wait = rclcpp::Duration::from_seconds(  1.0 );
           const auto end_time = start_time + wait;
           while (  rclcpp::Clock(   ).now(   ) < end_time )
           {
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           }
          
           // We may miss the last message since time allowed is exactly the time needed
           EXPECT_NEAR(  target_msg_count,   echo_received_counter,   1 );
          
           executor.cancel(   );
          }
          
          /**
           * @brief test_state_publish_rate Test that state publish rate matches configure rate
           */
     440  TEST_P(  TrajectoryControllerTestParameterized,   test_state_publish_rate )
          {
           test_state_publish_rate_target(  10 );
          }
          
     445  TEST_P(  TrajectoryControllerTestParameterized,   zero_state_publish_rate )
          {
           test_state_publish_rate_target(  0 );
          }
          
          /**
           * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from internal controller order
           */
     453  TEST_P(  TrajectoryControllerTestParameterized,   test_jumbled_joint_order )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {},   &executor );
           {
           trajectory_msgs::msg::JointTrajectory traj_msg;
           const std::vector<std::string> jumbled_joint_names{
           joint_names_[1],   joint_names_[2],   joint_names_[0]};
           traj_msg.joint_names = jumbled_joint_names;
           traj_msg.header.stamp = rclcpp::Time(  0 );
           traj_msg.points.resize(  1 );
          
           traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(  0.25 );
           traj_msg.points[0].positions.resize(  3 );
           traj_msg.points[0].positions[0] = 2.0;
           traj_msg.points[0].positions[1] = 3.0;
           traj_msg.points[0].positions[2] = 1.0;
          
           if (  traj_controller_->has_velocity_command_interface(   ) )
           {
           traj_msg.points[0].velocities.resize(  3 );
           traj_msg.points[0].velocities[0] = -0.1;
           traj_msg.points[0].velocities[1] = -0.1;
           traj_msg.points[0].velocities[2] = -0.1;
           }
          
           if (  traj_controller_->has_effort_command_interface(   ) )
           {
           traj_msg.points[0].effort.resize(  3 );
           traj_msg.points[0].effort[0] = -0.1;
           traj_msg.points[0].effort[1] = -0.1;
           traj_msg.points[0].effort[2] = -0.1;
           }
          
           trajectory_publisher_->publish(  traj_msg );
           }
          
           traj_controller_->wait_for_trajectory(  executor );
           // update for 0.25 seconds
           // TODO(  destogl ): Make this time a bit shorter to increase stability on the CI?
           // Currently COMMON_THRESHOLD is adjusted.
           updateController(  rclcpp::Duration::from_seconds(  0.25 ) );
          
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           EXPECT_NEAR(  1.0,   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  2.0,   joint_pos_[1],   COMMON_THRESHOLD );
           EXPECT_NEAR(  3.0,   joint_pos_[2],   COMMON_THRESHOLD );
           }
          
           if (  traj_controller_->has_velocity_command_interface(   ) )
           {
           EXPECT_GT(  0.0,   joint_vel_[0] );
           EXPECT_GT(  0.0,   joint_vel_[1] );
           EXPECT_GT(  0.0,   joint_vel_[2] );
           }
          
           if (  traj_controller_->has_effort_command_interface(   ) )
           {
           EXPECT_GT(  0.0,   joint_eff_[0] );
           EXPECT_GT(  0.0,   joint_eff_[1] );
           EXPECT_GT(  0.0,   joint_eff_[2] );
           }
          }
          
          /**
           * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints
           */
     521  TEST_P(  TrajectoryControllerTestParameterized,   test_partial_joint_list )
          {
           rclcpp::Parameter partial_joints_parameters(  "allow_partial_joints_goal",   true );
          
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {partial_joints_parameters},   &executor );
          
           const double initial_joint1_cmd = joint_pos_[0];
           const double initial_joint2_cmd = joint_pos_[1];
           const double initial_joint3_cmd = joint_pos_[2];
           trajectory_msgs::msg::JointTrajectory traj_msg;
          
           {
           std::vector<std::string> partial_joint_names{joint_names_[1],   joint_names_[0]};
           traj_msg.joint_names = partial_joint_names;
           traj_msg.header.stamp = rclcpp::Time(  0 );
           traj_msg.points.resize(  1 );
          
           traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(  0.25 );
           traj_msg.points[0].positions.resize(  2 );
           traj_msg.points[0].positions[0] = 2.0;
           traj_msg.points[0].positions[1] = 1.0;
           traj_msg.points[0].velocities.resize(  2 );
           traj_msg.points[0].velocities[0] =
           copysign(  2.0,   traj_msg.points[0].positions[0] - initial_joint2_cmd );
           traj_msg.points[0].velocities[1] =
           copysign(  1.0,   traj_msg.points[0].positions[1] - initial_joint1_cmd );
          
           trajectory_publisher_->publish(  traj_msg );
           }
          
           traj_controller_->wait_for_trajectory(  executor );
           // update for 0.5 seconds
           updateController(  rclcpp::Duration::from_seconds(  0.25 ) );
          
           double threshold = 0.001;
          
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           EXPECT_NEAR(  traj_msg.points[0].positions[1],   joint_pos_[0],   threshold );
           EXPECT_NEAR(  traj_msg.points[0].positions[0],   joint_pos_[1],   threshold );
           EXPECT_NEAR(  initial_joint3_cmd,   joint_pos_[2],   threshold )
           << "Joint 3 command should be current position";
           }
          
           if (  
           std::find(  command_interface_types_.begin(   ),   command_interface_types_.end(   ),   "velocity" ) !=
           command_interface_types_.end(   ) )
           {
           // estimate the sign of the velocity
           // joint rotates forward
           EXPECT_TRUE(  is_same_sign(  traj_msg.points[0].positions[0] - initial_joint2_cmd,   joint_vel_[0] ) );
           EXPECT_TRUE(  is_same_sign(  traj_msg.points[0].positions[1] - initial_joint1_cmd,   joint_vel_[1] ) );
           EXPECT_NEAR(  0.0,   joint_vel_[2],   threshold )
           << "Joint 3 velocity should be 0.0 since it's not in the goal";
           }
          
           if (  
           std::find(  command_interface_types_.begin(   ),   command_interface_types_.end(   ),   "effort" ) !=
           command_interface_types_.end(   ) )
           {
           // estimate the sign of the velocity
           // joint rotates forward
           EXPECT_TRUE(  is_same_sign(  traj_msg.points[0].positions[0] - initial_joint2_cmd,   joint_eff_[0] ) );
           EXPECT_TRUE(  is_same_sign(  traj_msg.points[0].positions[1] - initial_joint1_cmd,   joint_eff_[1] ) );
           EXPECT_NEAR(  0.0,   joint_eff_[2],   threshold )
           << "Joint 3 effort should be 0.0 since it's not in the goal";
           }
           // TODO(  anyone ): add here ckecks for acceleration commands
          
           executor.cancel(   );
          }
          
          /**
           * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints without allow_partial_joints_goal
           */
     597  TEST_P(  TrajectoryControllerTestParameterized,   test_partial_joint_list_not_allowed )
          {
           rclcpp::Parameter partial_joints_parameters(  "allow_partial_joints_goal",   false );
          
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {partial_joints_parameters},   &executor );
          
           const double initial_joint1_cmd = joint_pos_[0];
           const double initial_joint2_cmd = joint_pos_[1];
           const double initial_joint3_cmd = joint_pos_[2];
           const double initial_joint_vel = 0.0;
           const double initial_joint_acc = 0.0;
           trajectory_msgs::msg::JointTrajectory traj_msg;
          
           {
           std::vector<std::string> partial_joint_names{joint_names_[1],   joint_names_[0]};
           traj_msg.joint_names = partial_joint_names;
           traj_msg.header.stamp = rclcpp::Time(  0 );
           traj_msg.points.resize(  1 );
          
           traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(  0.25 );
           traj_msg.points[0].positions.resize(  2 );
           traj_msg.points[0].positions[0] = 2.0;
           traj_msg.points[0].positions[1] = 1.0;
           traj_msg.points[0].velocities.resize(  2 );
           traj_msg.points[0].velocities[0] = 2.0;
           traj_msg.points[0].velocities[1] = 1.0;
          
           trajectory_publisher_->publish(  traj_msg );
           }
          
           traj_controller_->wait_for_trajectory(  executor );
           // update for 0.5 seconds
           updateController(  rclcpp::Duration::from_seconds(  0.25 ) );
          
           double threshold = 0.001;
           EXPECT_NEAR(  initial_joint1_cmd,   joint_pos_[0],   threshold )
           << "All joints command should be current position because goal was rejected";
           EXPECT_NEAR(  initial_joint2_cmd,   joint_pos_[1],   threshold )
           << "All joints command should be current position because goal was rejected";
           EXPECT_NEAR(  initial_joint3_cmd,   joint_pos_[2],   threshold )
           << "All joints command should be current position because goal was rejected";
          
           if (  
           std::find(  command_interface_types_.begin(   ),   command_interface_types_.end(   ),   "velocity" ) !=
           command_interface_types_.end(   ) )
           {
           EXPECT_NEAR(  initial_joint_vel,   joint_vel_[0],   threshold )
           << "All joints velocities should be 0.0 because goal was rejected";
           EXPECT_NEAR(  initial_joint_vel,   joint_vel_[1],   threshold )
           << "All joints velocities should be 0.0 because goal was rejected";
           EXPECT_NEAR(  initial_joint_vel,   joint_vel_[2],   threshold )
           << "All joints velocities should be 0.0 because goal was rejected";
           }
          
           if (  
           std::find(  command_interface_types_.begin(   ),   command_interface_types_.end(   ),   "acceleration" ) !=
           command_interface_types_.end(   ) )
           {
           EXPECT_NEAR(  initial_joint_acc,   joint_acc_[0],   threshold )
           << "All joints accelerations should be 0.0 because goal was rejected";
           EXPECT_NEAR(  initial_joint_acc,   joint_acc_[1],   threshold )
           << "All joints accelerations should be 0.0 because goal was rejected";
           EXPECT_NEAR(  initial_joint_acc,   joint_acc_[2],   threshold )
           << "All joints accelerations should be 0.0 because goal was rejected";
           }
          
           executor.cancel(   );
          }
          
          /**
           * @brief invalid_message Test mismatched joint and reference vector sizes
           */
     670  TEST_P(  TrajectoryControllerTestParameterized,   invalid_message )
          {
           rclcpp::Parameter partial_joints_parameters(  "allow_partial_joints_goal",   false );
           rclcpp::Parameter allow_integration_parameters(  "allow_integration_in_goal_trajectories",   false );
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  
           true,   {partial_joints_parameters,   allow_integration_parameters},   &executor );
          
           trajectory_msgs::msg::JointTrajectory traj_msg,   good_traj_msg;
          
           good_traj_msg.joint_names = joint_names_;
           good_traj_msg.header.stamp = rclcpp::Time(  0 );
           good_traj_msg.points.resize(  1 );
           good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(  0.25 );
           good_traj_msg.points[0].positions.resize(  1 );
           good_traj_msg.points[0].positions = {1.0,   2.0,   3.0};
           good_traj_msg.points[0].velocities.resize(  1 );
           good_traj_msg.points[0].velocities = {-1.0,   -2.0,   -3.0};
           EXPECT_TRUE(  traj_controller_->validate_trajectory_msg(  good_traj_msg ) );
          
           // Incompatible joint names
           traj_msg = good_traj_msg;
           traj_msg.joint_names = {"bad_name"};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // No position data
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions.clear(   );
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few positions
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions = {1.0,   2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too many positions
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions = {1.0,   2.0,   3.0,   4.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few velocities
           traj_msg = good_traj_msg;
           traj_msg.points[0].velocities = {1.0,   2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few accelerations
           traj_msg = good_traj_msg;
           traj_msg.points[0].accelerations = {1.0,   2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few efforts
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions.clear(   );
           traj_msg.points[0].effort = {1.0,   2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Non-strictly increasing waypoint times
           traj_msg = good_traj_msg;
           traj_msg.points.push_back(  traj_msg.points.front(   ) );
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          }
          
          /// With allow_integration_in_goal_trajectories parameter trajectory missing position or velocities
          /// are accepted
     734  TEST_P(  TrajectoryControllerTestParameterized,   missing_positions_message_accepted )
          {
           rclcpp::Parameter allow_integration_parameters(  "allow_integration_in_goal_trajectories",   true );
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {allow_integration_parameters},   &executor );
          
           trajectory_msgs::msg::JointTrajectory traj_msg,   good_traj_msg;
          
           good_traj_msg.joint_names = joint_names_;
           good_traj_msg.header.stamp = rclcpp::Time(  0 );
           good_traj_msg.points.resize(  1 );
           good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(  0.25 );
           good_traj_msg.points[0].positions.resize(  1 );
           good_traj_msg.points[0].positions = {1.0,   2.0,   3.0};
           good_traj_msg.points[0].velocities.resize(  1 );
           good_traj_msg.points[0].velocities = {-1.0,   -2.0,   -3.0};
           good_traj_msg.points[0].accelerations.resize(  1 );
           good_traj_msg.points[0].accelerations = {1.0,   2.0,   3.0};
           EXPECT_TRUE(  traj_controller_->validate_trajectory_msg(  good_traj_msg ) );
          
           // No position data
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions.clear(   );
           EXPECT_TRUE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // No position and velocity data
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions.clear(   );
           traj_msg.points[0].velocities.clear(   );
           EXPECT_TRUE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // All empty
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions.clear(   );
           traj_msg.points[0].velocities.clear(   );
           traj_msg.points[0].accelerations.clear(   );
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few positions
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions = {1.0,   2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too many positions
           traj_msg = good_traj_msg;
           traj_msg.points[0].positions = {1.0,   2.0,   3.0,   4.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few velocities
           traj_msg = good_traj_msg;
           traj_msg.points[0].velocities = {1.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          
           // Incompatible data sizes,   too few accelerations
           traj_msg = good_traj_msg;
           traj_msg.points[0].accelerations = {2.0};
           EXPECT_FALSE(  traj_controller_->validate_trajectory_msg(  traj_msg ) );
          }
          
          /**
           * @brief test_trajectory_replace Test replacing an existing trajectory
           */
     796  TEST_P(  TrajectoryControllerTestParameterized,   test_trajectory_replace )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           rclcpp::Parameter partial_joints_parameters(  "allow_partial_joints_goal",   true );
           SetUpAndActivateTrajectoryController(  true,   {partial_joints_parameters},   &executor );
          
           subscribeToState(   );
          
           std::vector<std::vector<double>> points_old{{{2.,   3.,   4.}}};
           std::vector<std::vector<double>> points_old_velocities{{{0.2,   0.3,   0.4}}};
           std::vector<std::vector<double>> points_partial_new{{1.5}};
           std::vector<std::vector<double>> points_partial_new_velocities{{0.15}};
          
           const auto delay = std::chrono::milliseconds(  500 );
           builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(  delay )};
           publish(  time_from_start,   points_old,   rclcpp::Time(   ),   {},   points_old_velocities );
           trajectory_msgs::msg::JointTrajectoryPoint expected_actual,   expected_desired;
           expected_actual.positions = {points_old[0].begin(   ),   points_old[0].end(   )};
           expected_desired.positions = {points_old[0].begin(   ),   points_old[0].end(   )};
           expected_actual.velocities = {points_old_velocities[0].begin(   ),   points_old_velocities[0].end(   )};
           expected_desired.velocities = {points_old_velocities[0].begin(   ),   points_old_velocities[0].end(   )};
           // Check that we reached end of points_old trajectory
           // Denis: delta was 0.1 with 0.2 works for me
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.2 );
          
           RCLCPP_INFO(  traj_controller_->get_node(   )->get_logger(   ),   "Sending new trajectory" );
           points_partial_new_velocities[0][0] =
           copysign(  0.15,   points_partial_new[0][0] - joint_state_pos_[0] );
           publish(  time_from_start,   points_partial_new,   rclcpp::Time(   ),   {},   points_partial_new_velocities );
          
           // Replaced trajectory is a mix of previous and current goal
           expected_desired.positions[0] = points_partial_new[0][0];
           expected_desired.positions[1] = points_old[0][1];
           expected_desired.positions[2] = points_old[0][2];
           expected_desired.velocities[0] = points_partial_new_velocities[0][0];
           expected_desired.velocities[1] = 0.0;
           expected_desired.velocities[2] = 0.0;
           expected_actual = expected_desired;
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          }
          
          /**
           * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory
           */
     840  TEST_P(  TrajectoryControllerTestParameterized,   test_ignore_old_trajectory )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {},   &executor );
           subscribeToState(   );
          
           // TODO(  anyone ): add expectations for velocities and accelerations
           std::vector<std::vector<double>> points_old{{{2.,   3.,   4.},   {4.,   5.,   6.}}};
           std::vector<std::vector<double>> points_new{{{-1.,   -2.,   -3.}}};
          
           const auto delay = std::chrono::milliseconds(  500 );
           builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(  delay )};
           publish(  time_from_start,   points_old );
           trajectory_msgs::msg::JointTrajectoryPoint expected_actual,   expected_desired;
           expected_actual.positions = {points_old[0].begin(   ),   points_old[0].end(   )};
           expected_desired = expected_actual;
           // Check that we reached end of points_old[0] trajectory
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          
           RCLCPP_INFO(  traj_controller_->get_node(   )->get_logger(   ),   "Sending new trajectory in the past" );
           // New trajectory will end before current time
           rclcpp::Time new_traj_start = rclcpp::Clock(   ).now(   ) - delay - std::chrono::milliseconds(  100 );
           expected_actual.positions = {points_old[1].begin(   ),   points_old[1].end(   )};
           expected_desired = expected_actual;
           publish(  time_from_start,   points_new,   new_traj_start );
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          }
          
     868  TEST_P(  TrajectoryControllerTestParameterized,   test_ignore_partial_old_trajectory )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           SetUpAndActivateTrajectoryController(  true,   {},   &executor );
           subscribeToState(   );
          
           std::vector<std::vector<double>> points_old{{{2.,   3.,   4.},   {4.,   5.,   6.}}};
           std::vector<std::vector<double>> points_new{{{-1.,   -2.,   -3.},   {-2.,   -4.,   -6.}}};
          
           const auto delay = std::chrono::milliseconds(  500 );
           builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(  delay )};
           publish(  time_from_start,   points_old );
           trajectory_msgs::msg::JointTrajectoryPoint expected_actual,   expected_desired;
           expected_actual.positions = {points_old[0].begin(   ),   points_old[0].end(   )};
           expected_desired = expected_actual;
           // Check that we reached end of points_old[0]trajectory
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          
           RCLCPP_INFO(  
           traj_controller_->get_node(   )->get_logger(   ),   "Sending new trajectory partially in the past" );
           // New trajectory first point is in the past,   second is in the future
           rclcpp::Time new_traj_start = rclcpp::Clock(   ).now(   ) - delay - std::chrono::milliseconds(  100 );
           expected_actual.positions = {points_new[1].begin(   ),   points_new[1].end(   )};
           expected_desired = expected_actual;
           publish(  time_from_start,   points_new,   new_traj_start );
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          }
          
     896  TEST_P(  TrajectoryControllerTestParameterized,   test_execute_partial_traj_in_future )
          {
           SetUpTrajectoryController(   );
           auto traj_node = traj_controller_->get_node(   );
           RCLCPP_WARN(  
           traj_node->get_logger(   ),  
           "Test disabled until current_trajectory is taken into account when adding a new trajectory." );
           // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149
           return;
          
           // TODO(  anyone ): use SetUpAndActivateTrajectoryController method instead of the next line
           rclcpp::executors::SingleThreadedExecutor executor;
           executor.add_node(  traj_node->get_node_base_interface(   ) );
           subscribeToState(   );
           rclcpp::Parameter partial_joints_parameters(  "allow_partial_joints_goal",   true );
           traj_node->set_parameter(  partial_joints_parameters );
           traj_controller_->get_node(   )->configure(   );
           traj_controller_->get_node(   )->activate(   );
          
           std::vector<std::vector<double>> full_traj{{{2.,   3.,   4.},   {4.,   6.,   8.}}};
           std::vector<std::vector<double>> full_traj_velocities{{{0.2,   0.3,   0.4},   {0.4,   0.6,   0.8}}};
           std::vector<std::vector<double>> partial_traj{
           {{-1.,   -2.},  
           {
           -2.,  
           -4,  
           }}};
           std::vector<std::vector<double>> partial_traj_velocities{
           {{-0.1,   -0.2},  
           {
           -0.2,  
           -0.4,  
           }}};
           const auto delay = std::chrono::milliseconds(  500 );
           builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(  delay )};
           // Send full trajectory
           publish(  points_delay,   full_traj,   rclcpp::Time(   ),   {},   full_traj_velocities );
           // Sleep until first waypoint of full trajectory
          
           trajectory_msgs::msg::JointTrajectoryPoint expected_actual,   expected_desired;
           expected_actual.positions = {full_traj[0].begin(   ),   full_traj[0].end(   )};
           expected_desired = expected_actual;
           // Check that we reached end of points_old[0]trajectory and are starting points_old[1]
           waitAndCompareState(  expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay ),   0.1 );
          
           // Send partial trajectory starting after full trajecotry is complete
           RCLCPP_INFO(  traj_node->get_logger(   ),   "Sending new trajectory in the future" );
           publish(  
           points_delay,   partial_traj,   rclcpp::Clock(   ).now(   ) + delay * 2,   {},   partial_traj_velocities );
           // Wait until the end start and end of partial traj
          
           expected_actual.positions = {partial_traj.back(   )[0],   partial_traj.back(   )[1],   full_traj.back(   )[2]};
           expected_desired = expected_actual;
          
           waitAndCompareState(  
           expected_actual,   expected_desired,   executor,   rclcpp::Duration(  delay * (  2 + 2 ) ),   0.1 );
          }
          
     954  TEST_P(  TrajectoryControllerTestParameterized,   test_jump_when_state_tracking_error_updated )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           // default if false so it will not be actually set parameter
           rclcpp::Parameter is_open_loop_parameters(  "open_loop_control",   false );
           SetUpAndActivateTrajectoryController(  true,   {is_open_loop_parameters},   &executor,   true );
          
           // goal setup
           std::vector<double> first_goal = {3.3,   4.4,   5.5};
           std::vector<std::vector<double>> first_goal_velocities = {{0.33,   0.44,   0.55}};
           std::vector<double> second_goal = {6.6,   8.8,   11.0};
           std::vector<std::vector<double>> second_goal_velocities = {{0.66,   0.88,   1.1}};
           double state_from_command_offset = 0.3;
          
           // send msg
           builtin_interfaces::msg::Duration time_from_start;
           time_from_start.sec = 1;
           time_from_start.nanosec = 0;
           std::vector<std::vector<double>> points{{first_goal}};
           publish(  time_from_start,   points,   rclcpp::Time(   ),   {},   first_goal_velocities );
           traj_controller_->wait_for_trajectory(  executor );
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
          
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           // JTC is executing trajectory in open-loop therefore:
           // - internal state does not have to be updated (  in this test-case it shouldn't )
           // - internal command is updated
           EXPECT_NEAR(  INITIAL_POS_JOINT1,   joint_state_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
          
           // State interface should have offset from the command before starting a new trajectory
           joint_state_pos_[0] = first_goal[0] - state_from_command_offset;
          
           // Move joint further in the same direction as before (  to the second goal )
           points = {{second_goal}};
           publish(  time_from_start,   points,   rclcpp::Time(   ),   {},   second_goal_velocities );
           traj_controller_->wait_for_trajectory(  executor );
          
           // One the first update(  s ) there should be a "jump" in opposite direction from command
           // (  towards the state value )
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // Expect backward commands at first
           EXPECT_NEAR(  joint_state_pos_[0],   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_GT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_LT(  joint_pos_[0],   first_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_LT(  joint_pos_[0],   first_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_LT(  joint_pos_[0],   first_goal[0] );
          
           // Finally the second goal will be commanded/reached
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
           EXPECT_NEAR(  second_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
          
           // State interface should have offset from the command before starting a new trajectory
           joint_state_pos_[0] = second_goal[0] - state_from_command_offset;
          
           // Move joint back to the first goal
           points = {{first_goal}};
           publish(  time_from_start,   points );
           traj_controller_->wait_for_trajectory(  executor );
          
           // One the first update(  s ) there should be a "jump" in the goal direction from command
           // (  towards the state value )
           EXPECT_NEAR(  second_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // Expect backward commands at first
           EXPECT_NEAR(  joint_state_pos_[0],   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_LT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_LT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_LT(  joint_pos_[0],   joint_state_pos_[0] );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
          
           // Finally the first goal will be commanded/reached
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           }
          
           executor.cancel(   );
          }
          
    1043  TEST_P(  TrajectoryControllerTestParameterized,   test_no_jump_when_state_tracking_error_not_updated )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           // default if false so it will not be actually set parameter
           rclcpp::Parameter is_open_loop_parameters(  "open_loop_control",   true );
           SetUpAndActivateTrajectoryController(  true,   {is_open_loop_parameters},   &executor,   true );
          
           // goal setup
           std::vector<double> first_goal = {3.3,   4.4,   5.5};
           std::vector<double> second_goal = {6.6,   8.8,   11.0};
           double state_from_command_offset = 0.3;
          
           // send msg
           builtin_interfaces::msg::Duration time_from_start;
           time_from_start.sec = 1;
           time_from_start.nanosec = 0;
           std::vector<std::vector<double>> points{{first_goal}};
           publish(  time_from_start,   points );
           traj_controller_->wait_for_trajectory(  executor );
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
          
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           // JTC is executing trajectory in open-loop therefore:
           // - internal state does not have to be updated (  in this test-case it shouldn't )
           // - internal command is updated
           EXPECT_NEAR(  INITIAL_POS_JOINT1,   joint_state_pos_[0],   COMMON_THRESHOLD );
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
          
           // State interface should have offset from the command before starting a new trajectory
           joint_state_pos_[0] = first_goal[0] - state_from_command_offset;
          
           // Move joint further in the same direction as before (  to the second goal )
           points = {{second_goal}};
           publish(  time_from_start,   points );
           traj_controller_->wait_for_trajectory(  executor );
          
           // One the first update(  s ) there **should not** be a "jump" in opposite direction from command
           // (  towards the state value )
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // There should not be backward commands
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
          
           // Finally the second goal will be commanded/reached
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
           EXPECT_NEAR(  second_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
          
           // State interface should have offset from the command before starting a new trajectory
           joint_state_pos_[0] = second_goal[0] - state_from_command_offset;
          
           // Move joint back to the first goal
           points = {{first_goal}};
           publish(  time_from_start,   points );
           traj_controller_->wait_for_trajectory(  executor );
          
           // One the first update(  s ) there **should not** be a "jump" in the goal direction from command
           // (  towards the state value )
           EXPECT_NEAR(  second_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           // There should not be a jump toward commands
           EXPECT_NEAR(  second_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
           traj_controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) );
           EXPECT_GT(  joint_pos_[0],   first_goal[0] );
           EXPECT_LT(  joint_pos_[0],   second_goal[0] );
          
           // Finally the first goal will be commanded/reached
           updateController(  rclcpp::Duration::from_seconds(  1.1 ) );
           EXPECT_NEAR(  first_goal[0],   joint_pos_[0],   COMMON_THRESHOLD );
           }
          
           executor.cancel(   );
          }
          
          // Testing that values are read from state interfaces when hardware is started for the first
          // time and hardware state has offset --> this is indicated by NaN values in state interfaces
    1132  TEST_P(  TrajectoryControllerTestParameterized,   test_hw_states_has_offset_first_controller_start )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           // default if false so it will not be actually set parameter
           rclcpp::Parameter is_open_loop_parameters(  "open_loop_control",   true );
          
           // set command values to NaN
           for (  size_t i = 0; i < 3; ++i )
           {
           joint_pos_[i] = std::numeric_limits<double>::quiet_NaN(   );
           joint_vel_[i] = std::numeric_limits<double>::quiet_NaN(   );
           joint_acc_[i] = std::numeric_limits<double>::quiet_NaN(   );
           }
           SetUpAndActivateTrajectoryController(  true,   {is_open_loop_parameters},   &executor,   true );
          
           auto current_state_when_offset = traj_controller_->get_current_state_when_offset(   );
          
           for (  size_t i = 0; i < 3; ++i )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_state_pos_[i] );
          
           // check velocity
           if (  
           std::find(  
           state_interface_types_.begin(   ),   state_interface_types_.end(   ),  
           hardware_interface::HW_IF_VELOCITY ) != state_interface_types_.end(   ) &&
           std::find(  
           command_interface_types_.begin(   ),   command_interface_types_.end(   ),  
           hardware_interface::HW_IF_VELOCITY ) != command_interface_types_.end(   ) )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_state_pos_[i] );
           }
          
           // check acceleration
           if (  
           std::find(  
           state_interface_types_.begin(   ),   state_interface_types_.end(   ),  
           hardware_interface::HW_IF_ACCELERATION ) != state_interface_types_.end(   ) &&
           std::find(  
           command_interface_types_.begin(   ),   command_interface_types_.end(   ),  
           hardware_interface::HW_IF_ACCELERATION ) != command_interface_types_.end(   ) )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_state_pos_[i] );
           }
           }
          
           executor.cancel(   );
          }
          
          // Testing that values are read from state interfaces when hardware is started after some values
          // are set on the hardware commands
    1183  TEST_P(  TrajectoryControllerTestParameterized,   test_hw_states_has_offset_later_controller_start )
          {
           rclcpp::executors::SingleThreadedExecutor executor;
           // default if false so it will not be actually set parameter
           rclcpp::Parameter is_open_loop_parameters(  "open_loop_control",   true );
          
           // set command values to NaN
           for (  size_t i = 0; i < 3; ++i )
           {
           joint_pos_[i] = 3.1 + i;
           joint_vel_[i] = 0.25 + i;
           joint_acc_[i] = 0.02 + i / 10.0;
           }
           SetUpAndActivateTrajectoryController(  true,   {is_open_loop_parameters},   &executor,   true );
          
           auto current_state_when_offset = traj_controller_->get_current_state_when_offset(   );
          
           for (  size_t i = 0; i < 3; ++i )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_pos_[i] );
          
           // check velocity
           if (  
           std::find(  
           state_interface_types_.begin(   ),   state_interface_types_.end(   ),  
           hardware_interface::HW_IF_VELOCITY ) != state_interface_types_.end(   ) &&
           std::find(  
           command_interface_types_.begin(   ),   command_interface_types_.end(   ),  
           hardware_interface::HW_IF_VELOCITY ) != command_interface_types_.end(   ) )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_pos_[i] );
           }
          
           // check acceleration
           if (  
           std::find(  
           state_interface_types_.begin(   ),   state_interface_types_.end(   ),  
           hardware_interface::HW_IF_ACCELERATION ) != state_interface_types_.end(   ) &&
           std::find(  
           command_interface_types_.begin(   ),   command_interface_types_.end(   ),  
           hardware_interface::HW_IF_ACCELERATION ) != command_interface_types_.end(   ) )
           {
           EXPECT_EQ(  current_state_when_offset.positions[i],   joint_pos_[i] );
           }
           }
          
           executor.cancel(   );
          }
          
          // TODO(  andyz ): disabled because they started failing at the transition to Humble
          /*
          // position controllers
          INSTANTIATE_TEST_SUITE_P(  
           PositionTrajectoryControllers,   TrajectoryControllerTestParameterized,  
           ::testing::Values(  
           std::make_tuple(  std::vector<std::string>(  {"position"} ),   std::vector<std::string>(  {"position"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position"} ),   std::vector<std::string>(  {"position",   "velocity"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position"} ),  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ) ) ) );
          
          // position_velocity controllers
          INSTANTIATE_TEST_SUITE_P(  
           PositionVelocityTrajectoryControllers,   TrajectoryControllerTestParameterized,  
           ::testing::Values(  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity"} ),   std::vector<std::string>(  {"position"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity"} ),  
           std::vector<std::string>(  {"position",   "velocity"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity"} ),  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ) ) ) );
          
          // position_velocity_acceleration controllers
          INSTANTIATE_TEST_SUITE_P(  
           PositionVelocityAccelerationTrajectoryControllers,   TrajectoryControllerTestParameterized,  
           ::testing::Values(  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ),  
           std::vector<std::string>(  {"position"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ),  
           std::vector<std::string>(  {"position",   "velocity"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ),  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ) ) ) );
          
          // only velocity controller
          INSTANTIATE_TEST_SUITE_P(  
           OnlyVelocityTrajectoryControllers,   TrajectoryControllerTestParameterized,  
           ::testing::Values(  
           std::make_tuple(  
           std::vector<std::string>(  {"velocity"} ),   std::vector<std::string>(  {"position",   "velocity"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"velocity"} ),  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ) ) ) );
          
          // only effort controller
          INSTANTIATE_TEST_SUITE_P(  
           OnlyEffortTrajectoryControllers,   TrajectoryControllerTestParameterized,  
           ::testing::Values(  
           std::make_tuple(  
           std::vector<std::string>(  {"effort"} ),   std::vector<std::string>(  {"position",   "velocity"} ) ),  
           std::make_tuple(  
           std::vector<std::string>(  {"effort"} ),  
           std::vector<std::string>(  {"position",   "velocity",   "acceleration"} ) ) ) );
          */
          
    1293  TEST_F(  TrajectoryControllerTest,   incorrect_initialization_using_interface_parameters )
          {
           auto set_parameter_and_check_result = [&](   ) {
           EXPECT_EQ(  traj_controller_->get_state(   ).id(   ),   State::PRIMARY_STATE_UNCONFIGURED );
           SetParameters(   ); // This call is replacing the way parameters are set via launch
           traj_controller_->get_node(   )->configure(   );
           EXPECT_EQ(  traj_controller_->get_state(   ).id(   ),   State::PRIMARY_STATE_UNCONFIGURED );
           };
          
           SetUpTrajectoryController(  false );
          
           // command interfaces: empty
           command_interface_types_ = {};
           set_parameter_and_check_result(   );
          
           // command interfaces: bad_name
           command_interface_types_ = {"bad_name"};
           set_parameter_and_check_result(   );
          
           // command interfaces: effort has to be only
           command_interface_types_ = {"effort",   "position"};
           set_parameter_and_check_result(   );
          
           // command interfaces: velocity - position not present
           command_interface_types_ = {"velocity",   "acceleration"};
           set_parameter_and_check_result(   );
          
           // command interfaces: acceleration without position and velocity
           command_interface_types_ = {"acceleration"};
           set_parameter_and_check_result(   );
          
           // state interfaces: empty
           state_interface_types_ = {};
           set_parameter_and_check_result(   );
          
           // state interfaces: cannot not be effort
           state_interface_types_ = {"effort"};
           set_parameter_and_check_result(   );
          
           // state interfaces: bad name
           state_interface_types_ = {"bad_name"};
           set_parameter_and_check_result(   );
          
           // state interfaces: velocity - position not present
           state_interface_types_ = {"velocity"};
           set_parameter_and_check_result(   );
           state_interface_types_ = {"velocity",   "acceleration"};
           set_parameter_and_check_result(   );
          
           // state interfaces: acceleration without position and velocity
           state_interface_types_ = {"acceleration"};
           set_parameter_and_check_result(   );
          
           // velocity-only command interface: position - velocity not present
           command_interface_types_ = {"velocity"};
           state_interface_types_ = {"position"};
           set_parameter_and_check_result(   );
           state_interface_types_ = {"velocity"};
           set_parameter_and_check_result(   );
          
           // effort-only command interface: position - velocity not present
           command_interface_types_ = {"effort"};
           state_interface_types_ = {"position"};
           set_parameter_and_check_result(   );
           state_interface_types_ = {"velocity"};
           set_parameter_and_check_result(   );
          }

ros2_controllers/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

          // Copyright 2020 Open Source Robotics Foundation,   Inc.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_
          #define TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_
          
          #include <memory>
          #include <string>
          #include <tuple>
          #include <utility>
          #include <vector>
          
          #include "gtest/gtest.h"
          
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
          
          namespace
          {
          const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds?
          const double INITIAL_POS_JOINT1 = 1.1;
          const double INITIAL_POS_JOINT2 = 2.1;
          const double INITIAL_POS_JOINT3 = 3.1;
          const std::vector<double> INITIAL_POS_JOINTS = {
           INITIAL_POS_JOINT1,   INITIAL_POS_JOINT2,   INITIAL_POS_JOINT3};
          const std::vector<double> INITIAL_VEL_JOINTS = {0.0,   0.0,   0.0};
          const std::vector<double> INITIAL_ACC_JOINTS = {0.0,   0.0,   0.0};
          const std::vector<double> INITIAL_EFF_JOINTS = {0.0,   0.0,   0.0};
          } // namespace
          
          namespace test_trajectory_controllers
          {
      44  class TestableJointTrajectoryController
      45  : public joint_trajectory_controller::JointTrajectoryController
          {
          public:
           using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController;
           using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg;
          
      51   controller_interface::CallbackReturn on_configure(  
           const rclcpp_lifecycle::State & previous_state ) override
           {
           auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(  previous_state );
           // this class can still be useful without the wait set
           if (  joint_command_subscriber_ )
           {
           joint_cmd_sub_wait_set_.add_subscription(  joint_command_subscriber_ );
           }
           return ret;
           }
          
           /**
           * @brief wait_for_trajectory block until a new JointTrajectory is received.
           * Requires that the executor is not spinned elsewhere between the
           * message publication and the call to this function
           *
           * @return true if new JointTrajectory msg was received,   false if timeout
           */
      70   bool wait_for_trajectory(  
           rclcpp::Executor & executor,  
           const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500} )
           {
           bool success = joint_cmd_sub_wait_set_.wait(  timeout ).kind(   ) == rclcpp::WaitResultKind::Ready;
           if (  success )
           {
           executor.spin_some(   );
           }
           return success;
           }
          
      82   void set_joint_names(  const std::vector<std::string> & joint_names ) { joint_names_ = joint_names; }
          
      84   void set_command_interfaces(  const std::vector<std::string> & command_interfaces )
           {
           command_interface_types_ = command_interfaces;
           }
          
      89   void set_state_interfaces(  const std::vector<std::string> & state_interfaces )
           {
           state_interface_types_ = state_interfaces;
           }
          
      94   trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset(   )
           {
           return last_commanded_state_;
           }
          
      99   bool has_position_command_interface(   ) { return has_position_command_interface_; }
          
     101   bool has_velocity_command_interface(   ) { return has_velocity_command_interface_; }
          
     103   bool has_effort_command_interface(   ) { return has_effort_command_interface_; }
          
           rclcpp::WaitSet joint_cmd_sub_wait_set_;
          };
          
     108  class TrajectoryControllerTest : public ::testing::Test
          {
          public:
     111   static void SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
     113   virtual void SetUp(   )
           {
           controller_name_ = "test_joint_trajectory_controller";
          
           joint_names_ = {"joint1",   "joint2",   "joint3"};
           joint_pos_.resize(  joint_names_.size(   ),   0.0 );
           joint_state_pos_.resize(  joint_names_.size(   ),   0.0 );
           joint_vel_.resize(  joint_names_.size(   ),   0.0 );
           joint_state_vel_.resize(  joint_names_.size(   ),   0.0 );
           joint_acc_.resize(  joint_names_.size(   ),   0.0 );
           joint_state_acc_.resize(  joint_names_.size(   ),   0.0 );
           joint_eff_.resize(  joint_names_.size(   ),   0.0 );
           // Default interface values - they will be overwritten by parameterized tests
           command_interface_types_ = {"position"};
           state_interface_types_ = {"position",   "velocity"};
          
           node_ = std::make_shared<rclcpp::Node>(  "trajectory_publisher_" );
           trajectory_publisher_ = node_->create_publisher<trajectory_msgs::msg::JointTrajectory>(  
           controller_name_ + "/joint_trajectory",   rclcpp::SystemDefaultsQoS(   ) );
           }
          
     134   void SetUpTrajectoryController(  bool use_local_parameters = true )
           {
           traj_controller_ = std::make_shared<TestableJointTrajectoryController>(   );
           if (  use_local_parameters )
           {
           traj_controller_->set_joint_names(  joint_names_ );
           traj_controller_->set_command_interfaces(  command_interface_types_ );
           traj_controller_->set_state_interfaces(  state_interface_types_ );
           }
           auto ret = traj_controller_->init(  controller_name_ );
           if (  ret != controller_interface::return_type::OK )
           {
           FAIL(   );
           }
           }
          
     150   void SetParameters(   )
           {
           auto node = traj_controller_->get_node(   );
           const rclcpp::Parameter joint_names_param(  "joints",   joint_names_ );
           const rclcpp::Parameter cmd_interfaces_params(  "command_interfaces",   command_interface_types_ );
           const rclcpp::Parameter state_interfaces_params(  "state_interfaces",   state_interface_types_ );
           node->set_parameters(  {joint_names_param,   cmd_interfaces_params,   state_interfaces_params} );
           }
     158   void SetPidParameters(   )
           {
           auto node = traj_controller_->get_node(   );
          
           for (  size_t i = 0; i < joint_names_.size(   ); ++i )
           {
           const std::string prefix = "gains." + joint_names_[i];
           const rclcpp::Parameter k_p(  prefix + ".p",   0.0 );
           const rclcpp::Parameter k_i(  prefix + ".i",   0.0 );
           const rclcpp::Parameter k_d(  prefix + ".d",   0.0 );
           const rclcpp::Parameter i_clamp(  prefix + ".i_clamp",   0.0 );
           const rclcpp::Parameter ff_velocity_scale(  "ff_velocity_scale/" + joint_names_[i],   1.0 );
           node->set_parameters(  {k_p,   k_i,   k_d,   i_clamp,   ff_velocity_scale} );
           }
           }
          
     174   void SetUpAndActivateTrajectoryController(  
           bool use_local_parameters = true,   const std::vector<rclcpp::Parameter> & parameters = {},  
           rclcpp::Executor * executor = nullptr,   bool separate_cmd_and_state_values = false )
           {
           SetUpTrajectoryController(  use_local_parameters );
          
           for (  const auto & param : parameters )
           {
           traj_controller_->get_node(   )->set_parameter(  param );
           }
           if (  executor )
           {
           executor->add_node(  traj_controller_->get_node(   )->get_node_base_interface(   ) );
           }
          
           // ignore velocity tolerances for this test since they aren't committed in test_robot->write(   )
           rclcpp::Parameter stopped_velocity_parameters(  "constraints.stopped_velocity_tolerance",   0.0 );
           traj_controller_->get_node(   )->set_parameter(  stopped_velocity_parameters );
          
           // set pid parameters before configure
           SetPidParameters(   );
           traj_controller_->get_node(   )->configure(   );
          
           ActivateTrajectoryController(  separate_cmd_and_state_values );
           }
          
     200   void ActivateTrajectoryController(  bool separate_cmd_and_state_values = false )
           {
           std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
           std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
           pos_cmd_interfaces_.reserve(  joint_names_.size(   ) );
           vel_cmd_interfaces_.reserve(  joint_names_.size(   ) );
           acc_cmd_interfaces_.reserve(  joint_names_.size(   ) );
           eff_cmd_interfaces_.reserve(  joint_names_.size(   ) );
           pos_state_interfaces_.reserve(  joint_names_.size(   ) );
           vel_state_interfaces_.reserve(  joint_names_.size(   ) );
           acc_state_interfaces_.reserve(  joint_names_.size(   ) );
           for (  size_t i = 0; i < joint_names_.size(   ); ++i )
           {
           pos_cmd_interfaces_.emplace_back(  hardware_interface::CommandInterface(  
           joint_names_[i],   hardware_interface::HW_IF_POSITION,   &joint_pos_[i] ) );
           vel_cmd_interfaces_.emplace_back(  hardware_interface::CommandInterface(  
           joint_names_[i],   hardware_interface::HW_IF_VELOCITY,   &joint_vel_[i] ) );
           acc_cmd_interfaces_.emplace_back(  hardware_interface::CommandInterface(  
           joint_names_[i],   hardware_interface::HW_IF_ACCELERATION,   &joint_acc_[i] ) );
           eff_cmd_interfaces_.emplace_back(  hardware_interface::CommandInterface(  
           joint_names_[i],   hardware_interface::HW_IF_EFFORT,   &joint_eff_[i] ) );
          
           pos_state_interfaces_.emplace_back(  hardware_interface::StateInterface(  
           joint_names_[i],   hardware_interface::HW_IF_POSITION,  
           separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i] ) );
           vel_state_interfaces_.emplace_back(  hardware_interface::StateInterface(  
           joint_names_[i],   hardware_interface::HW_IF_VELOCITY,  
           separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i] ) );
           acc_state_interfaces_.emplace_back(  hardware_interface::StateInterface(  
           joint_names_[i],   hardware_interface::HW_IF_ACCELERATION,  
           separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i] ) );
          
           // Add to export lists and set initial values
           cmd_interfaces.emplace_back(  pos_cmd_interfaces_.back(   ) );
           cmd_interfaces.back(   ).set_value(  INITIAL_POS_JOINTS[i] );
           cmd_interfaces.emplace_back(  vel_cmd_interfaces_.back(   ) );
           cmd_interfaces.back(   ).set_value(  INITIAL_VEL_JOINTS[i] );
           cmd_interfaces.emplace_back(  acc_cmd_interfaces_.back(   ) );
           cmd_interfaces.back(   ).set_value(  INITIAL_ACC_JOINTS[i] );
           cmd_interfaces.emplace_back(  eff_cmd_interfaces_.back(   ) );
           cmd_interfaces.back(   ).set_value(  INITIAL_EFF_JOINTS[i] );
           joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
           joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
           joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
           state_interfaces.emplace_back(  pos_state_interfaces_.back(   ) );
           state_interfaces.emplace_back(  vel_state_interfaces_.back(   ) );
           state_interfaces.emplace_back(  acc_state_interfaces_.back(   ) );
           }
          
           traj_controller_->assign_interfaces(  std::move(  cmd_interfaces ),   std::move(  state_interfaces ) );
           traj_controller_->get_node(   )->activate(   );
           }
          
     253   static void TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
     255   void subscribeToState(   )
           {
           auto traj_lifecycle_node = traj_controller_->get_node(   );
           traj_lifecycle_node->set_parameter(  
           rclcpp::Parameter(  "state_publish_rate",   static_cast<double>(  100 ) ) );
          
           using control_msgs::msg::JointTrajectoryControllerState;
          
           auto qos = rclcpp::SensorDataQoS(   );
           // Needed,   otherwise spin_some(   ) returns only the oldest message in the queue
           // I do not understand why spin_some provides only one message
           qos.keep_last(  1 );
           state_subscriber_ = traj_lifecycle_node->create_subscription<JointTrajectoryControllerState>(  
           controller_name_ + "/state",   qos,   [&](  std::shared_ptr<JointTrajectoryControllerState> msg ) {
           std::lock_guard<std::mutex> guard(  state_mutex_ );
           state_msg_ = msg;
           } );
           }
          
           /// Publish trajectory msgs with multiple points
           /**
           * delay_btwn_points - delay between each points
           * points - vector of trajectories. One point per controlled joint
           * joint_names - names of joints,   if empty,   will use joint_names_ up to the number of provided points
           */
     280   void publish(  
           const builtin_interfaces::msg::Duration & delay_btwn_points,  
           const std::vector<std::vector<double>> & points,   rclcpp::Time start_time = rclcpp::Time(   ),  
           const std::vector<std::string> & joint_names = {},  
           const std::vector<std::vector<double>> & points_velocities = {} )
           {
           int wait_count = 0;
           const auto topic = trajectory_publisher_->get_topic_name(   );
           while (  node_->count_subscribers(  topic ) == 0 )
           {
           if (  wait_count >= 5 )
           {
           auto error_msg = std::string(  "publishing to " ) + topic + " but no node subscribes to it";
           throw std::runtime_error(  error_msg );
           }
           std::this_thread::sleep_for(  std::chrono::milliseconds(  100 ) );
           ++wait_count;
           }
          
           trajectory_msgs::msg::JointTrajectory traj_msg;
           if (  joint_names.empty(   ) )
           {
           traj_msg.joint_names = {joint_names_.begin(   ),   joint_names_.begin(   ) + points[0].size(   )};
           }
           else
           {
           traj_msg.joint_names = joint_names;
           }
           traj_msg.header.stamp = start_time;
           traj_msg.points.resize(  points.size(   ) );
          
           builtin_interfaces::msg::Duration duration_msg;
           duration_msg.sec = delay_btwn_points.sec;
           duration_msg.nanosec = delay_btwn_points.nanosec;
           rclcpp::Duration duration(  duration_msg );
           rclcpp::Duration duration_total(  duration_msg );
          
           for (  size_t index = 0; index < points.size(   ); ++index )
           {
           traj_msg.points[index].time_from_start = duration_total;
          
           traj_msg.points[index].positions.resize(  points[index].size(   ) );
           for (  size_t j = 0; j < points[index].size(   ); ++j )
           {
           traj_msg.points[index].positions[j] = points[index][j];
           }
           duration_total = duration_total + duration;
           }
          
           for (  size_t index = 0; index < points_velocities.size(   ); ++index )
           {
           traj_msg.points[index].velocities.resize(  points_velocities[index].size(   ) );
           for (  size_t j = 0; j < points_velocities[index].size(   ); ++j )
           {
           traj_msg.points[index].velocities[j] = points_velocities[index][j];
           }
           }
          
           trajectory_publisher_->publish(  traj_msg );
           }
          
     341   void updateController(  rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(  0.2 ) )
           {
           const auto start_time = rclcpp::Clock(   ).now(   );
           const auto end_time = start_time + wait_time;
           while (  rclcpp::Clock(   ).now(   ) < end_time )
           {
           traj_controller_->update(  rclcpp::Clock(   ).now(   ),   rclcpp::Clock(   ).now(   ) - start_time );
           }
           }
          
     351   void waitAndCompareState(  
     352   trajectory_msgs::msg::JointTrajectoryPoint expected_actual,  
     353   trajectory_msgs::msg::JointTrajectoryPoint expected_desired,   rclcpp::Executor & executor,  
     354   rclcpp::Duration controller_wait_time,   double allowed_delta )
           {
           {
           std::lock_guard<std::mutex> guard(  state_mutex_ );
           state_msg_.reset(   );
           }
           traj_controller_->wait_for_trajectory(  executor );
           updateController(  controller_wait_time );
           // Spin to receive latest state
           executor.spin_some(   );
           auto state_msg = getState(   );
           ASSERT_TRUE(  state_msg );
           for (  size_t i = 0; i < expected_actual.positions.size(   ); ++i )
           {
           SCOPED_TRACE(  "Joint " + std::to_string(  i ) );
           // TODO(  anyone ): add checking for velocties and accelerations
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           EXPECT_NEAR(  expected_actual.positions[i],   state_msg->actual.positions[i],   allowed_delta );
           }
           }
          
           for (  size_t i = 0; i < expected_desired.positions.size(   ); ++i )
           {
           SCOPED_TRACE(  "Joint " + std::to_string(  i ) );
           // TODO(  anyone ): add checking for velocties and accelerations
           if (  traj_controller_->has_position_command_interface(   ) )
           {
           EXPECT_NEAR(  expected_desired.positions[i],   state_msg->desired.positions[i],   allowed_delta );
           }
           }
           }
          
     387   std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> getState(   ) const
           {
           std::lock_guard<std::mutex> guard(  state_mutex_ );
           return state_msg_;
           }
     392   void test_state_publish_rate_target(  int target_msg_count );
          
     394   std::string controller_name_;
          
     396   std::vector<std::string> joint_names_;
     397   std::vector<std::string> command_interface_types_;
     398   std::vector<std::string> state_interface_types_;
          
     400   rclcpp::Node::SharedPtr node_;
     401   rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
          
     403   std::shared_ptr<TestableJointTrajectoryController> traj_controller_;
     404   rclcpp::Subscription<control_msgs::msg::JointTrajectoryControllerState>::SharedPtr
           state_subscriber_;
           mutable std::mutex state_mutex_;
           std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> state_msg_;
          
           std::vector<double> joint_pos_;
           std::vector<double> joint_vel_;
           std::vector<double> joint_acc_;
           std::vector<double> joint_eff_;
           std::vector<double> joint_state_pos_;
           std::vector<double> joint_state_vel_;
           std::vector<double> joint_state_acc_;
           std::vector<hardware_interface::CommandInterface> pos_cmd_interfaces_;
           std::vector<hardware_interface::CommandInterface> vel_cmd_interfaces_;
           std::vector<hardware_interface::CommandInterface> acc_cmd_interfaces_;
           std::vector<hardware_interface::CommandInterface> eff_cmd_interfaces_;
           std::vector<hardware_interface::StateInterface> pos_state_interfaces_;
           std::vector<hardware_interface::StateInterface> vel_state_interfaces_;
           std::vector<hardware_interface::StateInterface> acc_state_interfaces_;
          };
          
          // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest
     426  class TrajectoryControllerTestParameterized
     427  : public TrajectoryControllerTest,  
     428   public ::testing::WithParamInterface<
     429   std::tuple<std::vector<std::string>,   std::vector<std::string>>>
          {
          public:
           virtual void SetUp(   )
           {
           TrajectoryControllerTest::SetUp(   );
           command_interface_types_ = std::get<0>(  GetParam(   ) );
           state_interface_types_ = std::get<1>(  GetParam(   ) );
           }
          
           static void TearDownTestCase(   ) { TrajectoryControllerTest::TearDownTestCase(   ); }
          };
          
          } // namespace test_trajectory_controllers
          
          #endif // TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_

ros2_controllers/position_controllers/include/position_controllers/joint_group_position_controller.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_
          #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_
          
          #include <string>
          
          #include "forward_command_controller/forward_command_controller.hpp"
          #include "position_controllers/visibility_control.h"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          
          namespace position_controllers
          {
          /**
           * \brief Forward command controller for a set of position controlled joints (  linear or angular ).
           *
           * This class forwards the commanded positions down to a set of joints.
           *
           * \param joints Names of the joints to control.
           *
           * Subscribes to:
           * - \b commands (  std_msgs::msg::Float64MultiArray ) : The position commands to apply.
           */
      36  class JointGroupPositionController : public forward_command_controller::ForwardCommandController
          {
          public:
           POSITION_CONTROLLERS_PUBLIC
      40   JointGroupPositionController(   );
          
           POSITION_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init(   ) override;
          };
          
          } // namespace position_controllers
          
          #endif // POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_

ros2_controllers/position_controllers/include/position_controllers/visibility_control.h

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_
          #define POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define POSITION_CONTROLLERS_EXPORT __attribute__(  (  dllexport ) )
          #define POSITION_CONTROLLERS_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define POSITION_CONTROLLERS_EXPORT __declspec(  dllexport )
          #define POSITION_CONTROLLERS_IMPORT __declspec(  dllimport )
          #endif
          #ifdef POSITION_CONTROLLERS_BUILDING_DLL
          #define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_EXPORT
          #else
          #define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_IMPORT
          #endif
          #define POSITION_CONTROLLERS_PUBLIC_TYPE POSITION_CONTROLLERS_PUBLIC
          #define POSITION_CONTROLLERS_LOCAL
          #else
          #define POSITION_CONTROLLERS_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define POSITION_CONTROLLERS_IMPORT
          #if __GNUC__ >= 4
          #define POSITION_CONTROLLERS_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define POSITION_CONTROLLERS_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define POSITION_CONTROLLERS_PUBLIC
          #define POSITION_CONTROLLERS_LOCAL
          #endif
          #define POSITION_CONTROLLERS_PUBLIC_TYPE
          #endif
          
          #endif // POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_

ros2_controllers/position_controllers/src/joint_group_position_controller.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "position_controllers/joint_group_position_controller.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/parameter.hpp"
          
          namespace position_controllers
          {
      25  JointGroupPositionController::JointGroupPositionController(   )
          : forward_command_controller::ForwardCommandController(   )
          {
           interface_name_ = hardware_interface::HW_IF_POSITION;
          }
          
      31  controller_interface::CallbackReturn JointGroupPositionController::on_init(   )
          {
           auto ret = forward_command_controller::ForwardCommandController::on_init(   );
           if (  ret != CallbackReturn::SUCCESS )
           {
           return ret;
           }
          
           try
           {
           // Explicitly set the interface parameter declared by the forward_command_controller
           // to match the value set in the JointGroupPositionController constructor.
           get_node(   )->set_parameter(  
           rclcpp::Parameter(  "interface_name",   hardware_interface::HW_IF_POSITION ) );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           return CallbackReturn::SUCCESS;
          }
          } // namespace position_controllers
          
          #include "pluginlib/class_list_macros.hpp"
          
      58  PLUGINLIB_EXPORT_CLASS(  
           position_controllers::JointGroupPositionController,   controller_interface::ControllerInterface )

ros2_controllers/position_controllers/test/test_joint_group_position_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stddef.h>
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "test_joint_group_position_controller.hpp"
          
          using CallbackReturn = controller_interface::CallbackReturn;
          using hardware_interface::LoanedCommandInterface;
          
          namespace
          {
      35  rclcpp::WaitResultKind wait_for(  rclcpp::SubscriptionBase::SharedPtr subscription )
          {
           rclcpp::WaitSet wait_set;
           wait_set.add_subscription(  subscription );
           const auto timeout = std::chrono::seconds(  10 );
           return wait_set.wait(  timeout ).kind(   );
          }
          } // namespace
          
      44  void JointGroupPositionControllerTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
      46  void JointGroupPositionControllerTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
      48  void JointGroupPositionControllerTest::SetUp(   )
          {
           controller_ = std::make_unique<FriendJointGroupPositionController>(   );
          }
          
      53  void JointGroupPositionControllerTest::TearDown(   ) { controller_.reset(  nullptr ); }
          
      55  void JointGroupPositionControllerTest::SetUpController(   )
          {
           const auto result = controller_->init(  "test_joint_group_position_controller" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  joint_1_pos_cmd_ );
           command_ifs.emplace_back(  joint_2_pos_cmd_ );
           command_ifs.emplace_back(  joint_3_pos_cmd_ );
           controller_->assign_interfaces(  std::move(  command_ifs ),   {} );
          }
          
      67  TEST_F(  JointGroupPositionControllerTest,   JointsParameterNotSet )
          {
           SetUpController(   );
          
           // configure failed,   'joints' parameter not set
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      75  TEST_F(  JointGroupPositionControllerTest,   JointsParameterIsEmpty )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>(   )} );
          
           // configure failed,   'joints' is empty
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      84  TEST_F(  JointGroupPositionControllerTest,   ConfigureAndActivateParamsSuccess )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // configure successful
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          }
          
      94  TEST_F(  JointGroupPositionControllerTest,   ActivateWithWrongJointsNamesFails )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint4"}} );
          
           // activate failed,   'joint4' is not a valid joint name for the hardware
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint2"}} );
          
           // activate failed,   'acceleration' is not a registered interface for `joint1`
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
     110  TEST_F(  JointGroupPositionControllerTest,   CommandSuccessTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful though no command has been send yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands have been modified
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30.0 );
          }
          
     142  TEST_F(  JointGroupPositionControllerTest,   WrongCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // send command with wrong number of joints
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update failed,   command size does not match number of joints
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          }
          
     164  TEST_F(  JointGroupPositionControllerTest,   NoCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful,   no command received yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          }
          
     181  TEST_F(  JointGroupPositionControllerTest,   CommandCallbackTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // default values
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   3.1 );
          
           auto node_state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // send a new command
           rclcpp::Node test_node(  "test_node" );
           auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(  
           std::string(  controller_->get_node(   )->get_name(   ) ) + "/commands",   rclcpp::SystemDefaultsQoS(   ) );
           std_msgs::msg::Float64MultiArray command_msg;
           command_msg.data = {10.0,   20.0,   30.0};
           command_pub->publish(  command_msg );
          
           // wait for command message to be passed
           ASSERT_EQ(  wait_for(  controller_->joints_command_subscriber_ ),   rclcpp::WaitResultKind::Ready );
          
           // process callbacks
           rclcpp::spin_some(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_pos_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_pos_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_pos_cmd_.get_value(   ),   30.0 );
          }

ros2_controllers/position_controllers/test/test_joint_group_position_controller.hpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_
          #define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "position_controllers/joint_group_position_controller.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::HW_IF_POSITION;
          
          // subclassing and friending so we can access member variables
      32  class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController
          {
      34   FRIEND_TEST(  JointGroupPositionControllerTest,   CommandSuccessTest );
      35   FRIEND_TEST(  JointGroupPositionControllerTest,   WrongCommandCheckTest );
      36   FRIEND_TEST(  JointGroupPositionControllerTest,   CommandCallbackTest );
          };
          
      39  class JointGroupPositionControllerTest : public ::testing::Test
          {
          public:
      42   static void SetUpTestCase(   );
      43   static void TearDownTestCase(   );
          
      45   void SetUp(   );
      46   void TearDown(   );
          
      48   void SetUpController(   );
      49   void SetUpHandles(   );
          
          protected:
      52   std::unique_ptr<FriendJointGroupPositionController> controller_;
          
           // dummy joint state values used for tests
      55   const std::vector<std::string> joint_names_ = {"joint1",   "joint2",   "joint3"};
      56   std::vector<double> joint_commands_ = {1.1,   2.1,   3.1};
          
           CommandInterface joint_1_pos_cmd_{joint_names_[0],   HW_IF_POSITION,   &joint_commands_[0]};
      59   CommandInterface joint_2_pos_cmd_{joint_names_[1],   HW_IF_POSITION,   &joint_commands_[1]};
           CommandInterface joint_3_pos_cmd_{joint_names_[2],   HW_IF_POSITION,   &joint_commands_[2]};
          };
          
          #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_

ros2_controllers/position_controllers/test/test_load_joint_group_position_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gtest/gtest.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      23  TEST(  TestLoadJointGroupPositionController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_joint_group_position_controller",   "position_controllers/JointGroupPositionController" ) );
          
           rclcpp::shutdown(   );
          }

ros2_controllers/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
          #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
          
          #include <string>
          
          #include "forward_command_controller/forward_command_controller.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "velocity_controllers/visibility_control.h"
          
          namespace velocity_controllers
          {
          /**
           * \brief Forward command controller for a set of velocity controlled joints (  linear or angular ).
           *
           * This class forwards the commanded velocities down to a set of joints.
           *
           * \param joints Names of the joints to control.
           *
           * Subscribes to:
           * - \b command (  std_msgs::msg::Float64MultiArray ) : The velocity commands to apply.
           */
      36  class JointGroupVelocityController : public forward_command_controller::ForwardCommandController
          {
          public:
           VELOCITY_CONTROLLERS_PUBLIC
      40   JointGroupVelocityController(   );
          
           VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init(   ) override;
          
           VELOCITY_CONTROLLERS_PUBLIC
           controller_interface::CallbackReturn on_deactivate(  
           const rclcpp_lifecycle::State & previous_state ) override;
          };
          
          } // namespace velocity_controllers
          
          #endif // VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_

ros2_controllers/velocity_controllers/include/velocity_controllers/visibility_control.h

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          /* This header must be included by all rclcpp headers which declare symbols
           * which are defined in the rclcpp library. When not building the rclcpp
           * library,   i.e. when using the headers in other package's code,   the contents
           * of this header change the visibility of certain symbols which the rclcpp
           * library cannot have,   but the consuming code must have inorder to link.
           */
          
          #ifndef VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_
          #define VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_
          
          // This logic was borrowed (  then namespaced ) from the examples on the gcc wiki:
          // https://gcc.gnu.org/wiki/Visibility
          
          #if defined _WIN32 || defined __CYGWIN__
          #ifdef __GNUC__
          #define VELOCITY_CONTROLLERS_EXPORT __attribute__(  (  dllexport ) )
          #define VELOCITY_CONTROLLERS_IMPORT __attribute__(  (  dllimport ) )
          #else
          #define VELOCITY_CONTROLLERS_EXPORT __declspec(  dllexport )
          #define VELOCITY_CONTROLLERS_IMPORT __declspec(  dllimport )
          #endif
          #ifdef VELOCITY_CONTROLLERS_BUILDING_DLL
          #define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT
          #else
          #define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT
          #endif
          #define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC
          #define VELOCITY_CONTROLLERS_LOCAL
          #else
          #define VELOCITY_CONTROLLERS_EXPORT __attribute__(  (  visibility(  "default" ) ) )
          #define VELOCITY_CONTROLLERS_IMPORT
          #if __GNUC__ >= 4
          #define VELOCITY_CONTROLLERS_PUBLIC __attribute__(  (  visibility(  "default" ) ) )
          #define VELOCITY_CONTROLLERS_LOCAL __attribute__(  (  visibility(  "hidden" ) ) )
          #else
          #define VELOCITY_CONTROLLERS_PUBLIC
          #define VELOCITY_CONTROLLERS_LOCAL
          #endif
          #define VELOCITY_CONTROLLERS_PUBLIC_TYPE
          #endif
          
          #endif // VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_

ros2_controllers/velocity_controllers/src/joint_group_velocity_controller.cpp

       1  // Copyright 2020 PAL Robotics S.L.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <string>
          
          #include "controller_interface/controller_interface.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "rclcpp/logging.hpp"
          #include "rclcpp/parameter.hpp"
          #include "velocity_controllers/joint_group_velocity_controller.hpp"
          
          namespace velocity_controllers
          {
      25  JointGroupVelocityController::JointGroupVelocityController(   )
          : forward_command_controller::ForwardCommandController(   )
          {
           interface_name_ = hardware_interface::HW_IF_VELOCITY;
          }
          
      31  controller_interface::CallbackReturn JointGroupVelocityController::on_init(   )
          {
           auto ret = ForwardCommandController::on_init(   );
           if (  ret != CallbackReturn::SUCCESS )
           {
           return ret;
           }
          
           try
           {
           // Explicitly set the interface parameter declared by the forward_command_controller
           // to match the value set in the JointGroupVelocityController constructor.
           get_node(   )->set_parameter(  
           rclcpp::Parameter(  "interface_name",   hardware_interface::HW_IF_VELOCITY ) );
           }
           catch (  const std::exception & e )
           {
           fprintf(  stderr,   "Exception thrown during init stage with message: %s \n",   e.what(   ) );
           return CallbackReturn::ERROR;
           }
          
           return CallbackReturn::SUCCESS;
          }
          
      55  controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate(  
      56   const rclcpp_lifecycle::State & previous_state )
          {
           auto ret = ForwardCommandController::on_deactivate(  previous_state );
          
           // stop all joints
           for (  auto & command_interface : command_interfaces_ )
           {
           command_interface.set_value(  0.0 );
           }
          
           return ret;
          }
          
          } // namespace velocity_controllers
          
          #include "pluginlib/class_list_macros.hpp"
          
      73  PLUGINLIB_EXPORT_CLASS(  
           velocity_controllers::JointGroupVelocityController,   controller_interface::ControllerInterface )

ros2_controllers/velocity_controllers/test/test_joint_group_velocity_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <stddef.h>
          
          #include <functional>
          #include <memory>
          #include <string>
          #include <utility>
          #include <vector>
          
          #include "hardware_interface/loaned_command_interface.hpp"
          #include "hardware_interface/types/hardware_interface_return_values.hpp"
          #include "lifecycle_msgs/msg/state.hpp"
          #include "rclcpp/utilities.hpp"
          #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
          #include "test_joint_group_velocity_controller.hpp"
          
          using CallbackReturn = controller_interface::CallbackReturn;
          using hardware_interface::LoanedCommandInterface;
          
          namespace
          {
      35  rclcpp::WaitResultKind wait_for(  rclcpp::SubscriptionBase::SharedPtr subscription )
          {
           rclcpp::WaitSet wait_set;
           wait_set.add_subscription(  subscription );
           const auto timeout = std::chrono::seconds(  10 );
           return wait_set.wait(  timeout ).kind(   );
          }
          } // namespace
          
      44  void JointGroupVelocityControllerTest::SetUpTestCase(   ) { rclcpp::init(  0,   nullptr ); }
          
      46  void JointGroupVelocityControllerTest::TearDownTestCase(   ) { rclcpp::shutdown(   ); }
          
      48  void JointGroupVelocityControllerTest::SetUp(   )
          {
           controller_ = std::make_unique<FriendJointGroupVelocityController>(   );
          }
          
      53  void JointGroupVelocityControllerTest::TearDown(   ) { controller_.reset(  nullptr ); }
          
      55  void JointGroupVelocityControllerTest::SetUpController(   )
          {
           const auto result = controller_->init(  "test_joint_group_velocity_controller" );
           ASSERT_EQ(  result,   controller_interface::return_type::OK );
          
           std::vector<LoanedCommandInterface> command_ifs;
           command_ifs.emplace_back(  joint_1_cmd_ );
           command_ifs.emplace_back(  joint_2_cmd_ );
           command_ifs.emplace_back(  joint_3_cmd_ );
           controller_->assign_interfaces(  std::move(  command_ifs ),   {} );
          }
          
      67  TEST_F(  JointGroupVelocityControllerTest,   JointsParameterNotSet )
          {
           SetUpController(   );
          
           // configure failed,   'joints' parameter not set
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      75  TEST_F(  JointGroupVelocityControllerTest,   JointsParameterIsEmpty )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>(   )} );
          
           // configure failed,   'joints' is empty
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
      84  TEST_F(  JointGroupVelocityControllerTest,   ConfigureAndActivateParamsSuccess )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // configure successful
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          }
          
      94  TEST_F(  JointGroupVelocityControllerTest,   ActivateWithWrongJointsNamesFails )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint4"}} );
          
           // activate failed,   'joint4' is not a valid joint name for the hardware
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          
           controller_->get_node(   )->set_parameter(  {"joints",   std::vector<std::string>{"joint1",   "joint2"}} );
          
           // activate failed,   'acceleration' is not a registered interface for `joint1`
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
           ASSERT_EQ(  controller_->on_activate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::ERROR );
          }
          
     110  TEST_F(  JointGroupVelocityControllerTest,   CommandSuccessTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful though no command has been send yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           // send command
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0,   30.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update successful,   command received
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands have been modified
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   30.0 );
          }
          
     142  TEST_F(  JointGroupVelocityControllerTest,   WrongCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // send command with wrong number of joints
           auto command_ptr = std::make_shared<forward_command_controller::CmdType>(   );
           command_ptr->data = {10.0,   20.0};
           controller_->rt_command_ptr_.writeFromNonRT(  command_ptr );
          
           // update failed,   command size does not match number of joints
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::ERROR );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          }
          
     164  TEST_F(  JointGroupVelocityControllerTest,   NoCommandCheckTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // update successful,   no command received yet
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          }
          
     181  TEST_F(  JointGroupVelocityControllerTest,   CommandCallbackTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // default values
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           auto node_state = controller_->get_node(   )->configure(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
          
           node_state = controller_->get_node(   )->activate(   );
           ASSERT_EQ(  node_state.id(   ),   lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
          
           // send a new command
           rclcpp::Node test_node(  "test_node" );
           auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(  
           std::string(  controller_->get_node(   )->get_name(   ) ) + "/commands",   rclcpp::SystemDefaultsQoS(   ) );
           std_msgs::msg::Float64MultiArray command_msg;
           command_msg.data = {10.0,   20.0,   30.0};
           command_pub->publish(  command_msg );
          
           // wait for command message to be passed
           ASSERT_EQ(  wait_for(  controller_->joints_command_subscriber_ ),   rclcpp::WaitResultKind::Ready );
          
           // process callbacks
           rclcpp::spin_some(  controller_->get_node(   )->get_node_base_interface(   ) );
          
           // update successful
           ASSERT_EQ(  
           controller_->update(  rclcpp::Time(  0 ),   rclcpp::Duration::from_seconds(  0.01 ) ),  
           controller_interface::return_type::OK );
          
           // check command in handle was set
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   10.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   20.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   30.0 );
          }
          
     222  TEST_F(  JointGroupVelocityControllerTest,   StopJointsOnDeactivateTest )
          {
           SetUpController(   );
           controller_->get_node(   )->set_parameter(  {"joints",   joint_names_} );
          
           // configure successful
           ASSERT_EQ(  controller_->on_configure(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // check joint commands are still the default ones
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   1.1 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   2.1 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   3.1 );
          
           // stop the controller
           ASSERT_EQ(  controller_->on_deactivate(  rclcpp_lifecycle::State(   ) ),   CallbackReturn::SUCCESS );
          
           // check joint commands are now zero
           ASSERT_EQ(  joint_1_cmd_.get_value(   ),   0.0 );
           ASSERT_EQ(  joint_2_cmd_.get_value(   ),   0.0 );
           ASSERT_EQ(  joint_3_cmd_.get_value(   ),   0.0 );
          }

ros2_controllers/velocity_controllers/test/test_joint_group_velocity_controller.hpp

          // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
          #define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
          
          #include <memory>
          #include <string>
          #include <vector>
          
          #include "gmock/gmock.h"
          
          #include "hardware_interface/handle.hpp"
          #include "hardware_interface/types/hardware_interface_type_values.hpp"
          #include "velocity_controllers/joint_group_velocity_controller.hpp"
          
          using hardware_interface::CommandInterface;
          using hardware_interface::HW_IF_VELOCITY;
          
          // subclassing and friending so we can access member variables
      32  class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController
          {
      34   FRIEND_TEST(  JointGroupVelocityControllerTest,   CommandSuccessTest );
      35   FRIEND_TEST(  JointGroupVelocityControllerTest,   WrongCommandCheckTest );
      36   FRIEND_TEST(  JointGroupVelocityControllerTest,   CommandCallbackTest );
      37   FRIEND_TEST(  JointGroupVelocityControllerTest,   StopJointsOnDeactivateTest );
          };
          
      40  class JointGroupVelocityControllerTest : public ::testing::Test
          {
          public:
      43   static void SetUpTestCase(   );
      44   static void TearDownTestCase(   );
          
      46   void SetUp(   );
      47   void TearDown(   );
          
      49   void SetUpController(   );
          
          protected:
      52   std::unique_ptr<FriendJointGroupVelocityController> controller_;
          
           // dummy joint state values used for tests
      55   const std::vector<std::string> joint_names_ = {"joint1",   "joint2",   "joint3"};
      56   std::vector<double> joint_commands_ = {1.1,   2.1,   3.1};
          
           CommandInterface joint_1_cmd_{joint_names_[0],   HW_IF_VELOCITY,   &joint_commands_[0]};
      59   CommandInterface joint_2_cmd_{joint_names_[1],   HW_IF_VELOCITY,   &joint_commands_[1]};
           CommandInterface joint_3_cmd_{joint_names_[2],   HW_IF_VELOCITY,   &joint_commands_[2]};
          };
          
          #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_

ros2_controllers/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp

       1  // Copyright 2020 PAL Robotics SL.
          //
          // Licensed under the Apache License,   Version 2.0 (  the "License" );
          // you may not use this file except in compliance with the License.
          // You may obtain a copy of the License at
          //
          // http://www.apache.org/licenses/LICENSE-2.0
          //
          // Unless required by applicable law or agreed to in writing,   software
          // distributed under the License is distributed on an "AS IS" BASIS,  
          // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,   either express or implied.
          // See the License for the specific language governing permissions and
          // limitations under the License.
          
          #include <gmock/gmock.h>
          #include <memory>
          
          #include "controller_manager/controller_manager.hpp"
          #include "hardware_interface/resource_manager.hpp"
          #include "rclcpp/executors/single_threaded_executor.hpp"
          #include "ros2_control_test_assets/descriptions.hpp"
          
      23  TEST(  TestLoadJointGroupVelocityController,   load_controller )
          {
           rclcpp::init(  0,   nullptr );
          
           std::shared_ptr<rclcpp::Executor> executor =
           std::make_shared<rclcpp::executors::SingleThreadedExecutor>(   );
          
           controller_manager::ControllerManager cm(  
           std::make_unique<hardware_interface::ResourceManager>(  
           ros2_control_test_assets::minimal_robot_urdf ),  
           executor,   "test_controller_manager" );
          
           ASSERT_NO_THROW(  cm.load_controller(  
           "test_joint_group_velocity_controller",   "velocity_controllers/JointGroupVelocityController" ) );
          
           rclcpp::shutdown(   );
          }